different between rospy and roscpp on move_base_simple/goal
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 <ros ros.h=""> #include <geometry_msgs pose.h=""> #include <geometry_msgs posestamped.h=""> #include <move_base_msgs movebaseactionresult.h="">
//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<geometry_msgs::PoseStamped>("/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 ...