hi there:
recently, i have been working on publishing goals with move_base_simple/goal to make my robot move around directly. yesterday, i wrote something in C, trying to make the robot move from (0,0) - (3,0) - (0,0) - (0,4), but after it got (3,0), (0,0) was published, but before the robot got the origin place, i got the "goal reached" message from /move_base/result, which leading the robot move to the next goal, (0,4) i mean. today, i try the same thing in rospy, but everything works fine. can anyone tell me what is going on? btw, i am using ros kinetic with turtlebot, and i got the same question on turtlebot_gazebo.
here are the codes:
#include
#include
#include
#include
//ros publisher
ros::Publisher target_pub;
float place[4][2] = {{0.0, 1.0},{0.0, 0.0},{1.0, 0.0},{0.0, 0.0}};
int i = 0, j = 0;
void result_callback(const move_base_msgs::MoveBaseActionResult::ConstPtr &msg)
{
if(msg->status.status == 3)
i=i+1;
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "test_node");
ros::NodeHandle n;
ros::Subscriber result_sub = n.subscribe("/move_base/result", 10, result_callback);
target_pub = n.advertise("/move_base_simple/goal", 10);
ros::Rate loop_rate(1);
while (ros::ok())
{
if (i == 0)
{
geometry_msgs::PoseStamped msg;
msg.header.seq = j++;
msg.header.stamp = ros::Time::now();
msg.header.frame_id = "map";
msg.pose.position.x = place[0][0];
msg.pose.position.y = place[0][1];
msg.pose.position.z = 0.0;
msg.pose.orientation.w = 1.0;
msg.pose.orientation.x = 0.0;
msg.pose.orientation.y = 0.0;
msg.pose.orientation.z = 0.0;
target_pub.publish(msg);
}
else if (i < 4)
{
geometry_msgs::PoseStamped msg;
msg.header.seq = j++;
msg.header.stamp = ros::Time::now();
msg.header.frame_id = "map";
msg.pose.position.x = place[i][0];
msg.pose.position.y = place[i][1];
msg.pose.position.z = 0.0;
msg.pose.orientation.w = 1.0;
msg.pose.orientation.x = 0.0;
msg.pose.orientation.y = 0.0;
msg.pose.orientation.z = 0.0;
target_pub.publish(msg);
}
ros::spinOnce();
loop_rate.sleep();
}
return 0;
}
#!/usr/bin/env python
import rospy
from move_base_msgs.msg import MoveBaseActionResult
from geometry_msgs.msg import PoseStamped, Pose
from std_msgs.msg import Header
i = 0
j = 0
place = [[0.0, 1.0],[0.0, 0.0],[1.0, 0.0],[0.0, 0.0]]
def callback(data):
global i
if data.status.status == 3:
i=i+1
rospy.init_node('test_node', anonymous=True)
rospy.Subscriber("/move_base/result", MoveBaseActionResult, callback)
pub = rospy.Publisher("/move_base_simple/goal", PoseStamped, queue_size=10)
rate = rospy.Rate(1)
while not rospy.is_shutdown():
if i == 0:
msg = PoseStamped()
msg.header.seq = j
j=j+1
msg.header.stamp = rospy.Time.now()
msg.header.frame_id = "map"
msg.pose.position.x = place[0][0]
msg.pose.position.y = place[0][1]
msg.pose.position.z = 0.0
msg.pose.orientation.w = 1.0
msg.pose.orientation.x = 0.0
msg.pose.orientation.y = 0.0
msg.pose.orientation.z = 0.0
pub.publish(msg)
elif i<4:
msg = PoseStamped()
msg.header.seq = j
j=j+1
msg.header.stamp = rospy.Time.now()
msg.header.frame_id = "map"
msg.pose.position.x = place[i][0]
msg.pose.position.y = place[i][1]
msg.pose.position.z = 0.0
msg.pose.orientation.w = 1.0
msg.pose.orientation.x = 0.0
msg.pose.orientation.y = 0.0
msg.pose.orientation.z = 0.0
pub.publish(msg)
rate.sleep()
↧