Trying to communicate with robot in gazebo
Since, i am working on mobile manipulation and i am trying to command robot in gazebo. Manually, sending the joint commands on the topic, its moving and even with my c++ code. I have a Cartesian path planner, which plans the trajectory and i can visualize it in the rviz and it publishes on the topic with sensor_msgs/JointState and the type of topic to command the robot is trajectory_msgs/JointTrajectory. Since,
- I know i have to write a c++ script to subscribe to node from Cartesian planner and publishes it on the robot command topic. i have written the script but something is wrong i can see connection between theses two topic.
Code:
using namespace std;
sensor_msgs::JointState JS;
void jointStateCallback(const sensor_msgs::JointState msg) { JS = msg; }
int main(int argc, char** argv) {
ros::init(argc, argv, "mover_node");
ros::NodeHandle n;
ros::AsyncSpinner spinner(1);
spinner.start();
bool success;
ros::Publisher joint_pub = n.advertise<trajectory_msgs::JointTrajectory>("/panda/panda_arm_controller/command", 1, true);
ros::Subscriber joint_sub = n.subscribe<sensor_msgs::JointState>("/catersian/solution", 1, jointStateCallback);
trajectory_msgs::JointTrajectory JT;
JT.joint_names.clear();
JT.joint_names.push_back("panda_joint_x");
JT.joint_names.push_back("panda_joint_y");
JT.joint_names.push_back("panda_joint_theta");
JT.joint_names.push_back("schunk_pw70_joint_pan");
JT.joint_names.push_back("schunk_pw70_joint_tilt");
JT.joint_names.push_back("panda_joint_1");
JT.joint_names.push_back("panda_joint_2");
JT.joint_names.push_back("panda_joint_3");
JT.joint_names.push_back("panda_joint_4");
JT.joint_names.push_back("panda_joint_5");
JT.joint_names.push_back("panda_joint_6");
JT.joint_names.push_back("panda_joint_7");
JT.points.resize(2);
JT.points[0].positions.resize(JT.joint_names.size());
JT.points[1].positions.resize(JT.joint_names.size());
while(ros::ok()) {
if(JS.position.size() != 0)
{
cout << JT << endl;
JT.points[0].time_from_start = ros::Duration(0.0001);
JT.points[0].positions = JS.position;
JT.points[1].positions[0] = 0.0;
JT.points[1].positions[1] = 0.0;
JT.points[1].positions[2] = 0.0;
JT.points[1].positions[3] = 0.0;
JT.points[1].positions[4] = 0.0;
JT.points[1].positions[5] = 0.0;
JT.points[1].positions[6] = 0.0;
JT.points[1].positions[7] = 0.0;
JT.points[1].positions[8] = 0.0;
JT.points[1].positions[9] = 0.0;
JT.points[1].positions[10] = 0.0;
JT.points[1].positions[11] = 0.0;
JT.points[1].time_from_start = ros::Duration(1);
joint_pub.publish(JT);
}
}
ros::shutdown();
return 0;
}
Can you elaborate on the problems you're having. Does this code compile, or not. Does it produce error messages. Or does it not behave the way you expect it to?
Dear Peter, thanks for your reply. Actually, i have following concerns;
I believe that, i have not written the code according to the requirements. Can you please guide me, how to achieve this task.
Thank You
I've given you some pointers below.