moving mobile robot in zigzag way
Hello,
I want to move the husky robot using a code that publishes the command velocities. the movement should be like zigzag. here is the code that I am trying but it is not working.. I did not knew how to do it. Any help please ?
//const double M_PI=3.14159265359 ;
const double deg_to_rad = M_PI / 180.0 ;
class test_pf
{
public:
test_pf(ros::NodeHandle & n_) : n(n_)
{
vel_pub = n.advertise<geometry_msgs::Twist>("/husky_ns/husky/cmd_vel", 100);
};
void move(double val)
{
geometry_msgs::Twist msg;
msg.linear.x = val;
msg.linear.y = 0.0 ;
msg.linear.z = 0.0;
msg.angular.x = 0 ;
msg.angular.y = 0 ;
msg.angular.z = 0 ;
vel_pub.publish(msg);
}
void turn(double val)
{
geometry_msgs::Twist msg;
msg.linear.x = 0.0;
msg.linear.y = 0.0 ;
msg.linear.z = 0.0;
msg.angular.x = 0.0 ;
msg.angular.y = 0.0 ;
msg.angular.z = val ;
vel_pub.publish(msg);
}
private:
// ROS
ros::NodeHandle n;
ros::Publisher vel_pub ;
} ;
int main(int argc, char **argv)
{
// ROS node
std:: cout << " MAIN " << std::endl ;
ros::init(argc, argv, "move_robot");
ros::NodeHandle n;
ros::NodeHandle n_priv("~");
ros::Rate loop_rate(50);
// create an object
test_pf move_robot_obj(n);
while(ros::ok())
{
move_robot_obj.move(1.0);
loop_rate.sleep();
move_robot_obj.turn(4.0);
loop_rate.sleep();
move_robot_obj.move(1.0);
loop_rate.sleep();
move_robot_obj.turn(-4.0);
loop_rate.sleep();
}
return 0;
}
What behavior do you see when you try it? Does anything interesting (e.g., a warning or error) show up in the console?
no error in console.. it keeps moving forward and I cant notice the turns but eventually the track is changing slowly downward in un-noticable way