I would like to move my turtlebot forward for 1 meter (in the direction he is facing). The way i do it is like this:
- subscribe to topic "/camera/depth_registered/points"
- create a "fake point" (just like kinect would return. Im creating the fake point just to see if the point calculation is correct.)
- transform the point into /map space
- publish the point to /move_base_simple/goal
The code:
geometry_msgs::PointStamped p;
geometry_msgs::PointStamped p1;
p.header.stamp = ros::Time();
std::string frame1 = "/camera_depth_optical_frame";
p.header.frame_id = frame1.c_str();
p.point.x = 0;
p.point.y = 0;
p.point.z = 1; // 1 meter
std::string frame = "map";
try
{
listener.transformPoint(frame,p,p1);
}catch(tf::TransformException& ex) { ROS_ERROR("exception while transforming..."); }
// create message for move_base_simple/goal
geometry_msgs::PoseStamped msg;
msg.header.stamp = ros::Time();
std::string frame = "/map";
msg.header.frame_id = frame.c_str();
msg.pose.position = p1.point;
msg.pose.orientation = tf::createQuaternionMsgFromYaw(0.0);
publisher.publish(msg);
The problem is when i visualize the goal in rviz it is wrong so the robot moves into the wrong way. Can someone please explain how to fix this i'm struggling with this for days and im quite desperate.
Thanks!
↧