Skip to content
Snippets Groups Projects
Commit 04c52def authored by Nikhil Ambardar's avatar Nikhil Ambardar
Browse files

Update robot_models_node.cpp

parent 95a40135
No related branches found
No related tags found
No related merge requests found
Show whitespace changes
Inline Side-by-side
src/robot_models_node.cpp
+ 103
− 21
View file @ 04c52def
...@@ -55,19 +55,27 @@ ...@@ -55,19 +55,27 @@
worldobject o10; worldobject o10;
//o5a.name = "panda_joint1"; o5a.name = "panda_joint1";
//o5b.name = "panda_joint2"; o5b.name = "panda_joint2";
//o5c.name = "panda_joint3"; o5c.name = "panda_joint3";
//o5d.name = "panda_joint4"; o5d.name = "panda_joint4";
//o5e.name = "panda_joint5"; o5e.name = "panda_joint5";
//o5f.name = "panda_joint6"; o5f.name = "panda_joint6";
//o5g.name = "panda_joint7"; o5g.name = "panda_joint7";
o5a.x = 1.7; o5a.or2 = {1,5,6,7};
o5b.y = 1.8; o5b.or2 = {2,3,5,4};
o5c.z = 1.9; o5c.or2 = {5,1,9,6};
o5d.w = 2.0; o5d.or2 = {6,3,8,9};
o5a.p1 = {12, 11, 9, 18}; o5e.or2 = {8,2,7,5};
o5f.or2 = {7,4,6,4};
o5g.or2 = {9,5,5,8};
o5a.p1 = {1, 10, 9};
o5b.p1 = {14, 11, 6};
o5c.p1 = {4, 13, 7};
o5d.p1 = {2, 12, 1};
o5e.p1 = {3, 16, 5};
o5f.p1 = {11, 15, 3};
o5g.p1 = {5, 18, 4};
o8.robname = "Robot FPE"; o8.robname = "Robot FPE";
o8.of = true; o8.of = true;
...@@ -96,6 +104,8 @@ ...@@ -96,6 +104,8 @@
o6.z = 1.6; o6.z = 1.6;
o6.w = 1.9; o6.w = 1.9;
//ros::Publisher turtle_vel
//=node.advertise<geometry_msgs::Twist>("turtle2/cmd_vel", 10);
tf2_ros::Buffer tfBuffer; tf2_ros::Buffer tfBuffer;
tf2_ros::TransformListener tfListener(tfBuffer); tf2_ros::TransformListener tfListener(tfBuffer);
...@@ -107,20 +117,92 @@ ...@@ -107,20 +117,92 @@
{ {
geometry_msgs::TransformStamped transformStamped; geometry_msgs::TransformStamped transformStamped;
try { try {
transformStamped = tfBuffer.lookupTransform("panda_joint1", o5a.name, ros::Time(0)); transformStamped = tfBuffer.lookupTransform("world", o5a.name, ros::Time(0));
transformStamped = tfBuffer.lookupTransform("panda_joint2", o5b.name, ros::Time(0)); transformStamped = tfBuffer.lookupTransform("world", o5b.name, ros::Time(0));
transformStamped = tfBuffer.lookupTransform("panda_joint3", o5c.name, ros::Time(0)); transformStamped = tfBuffer.lookupTransform("world", o5c.name, ros::Time(0));
transformStamped = tfBuffer.lookupTransform("panda_joint4", o5d.name, ros::Time(0)); transformStamped = tfBuffer.lookupTransform("world", o5d.name, ros::Time(0));
transformStamped = tfBuffer.lookupTransform("panda_joint5", o5e.name, ros::Time(0)); transformStamped = tfBuffer.lookupTransform("world", o5e.name, ros::Time(0));
transformStamped = tfBuffer.lookupTransform("panda_joint6", o5f.name, ros::Time(0)); transformStamped = tfBuffer.lookupTransform("world", o5f.name, ros::Time(0));
transformStamped = tfBuffer.lookupTransform("panda_joint7", o5g.name, ros::Time(0)); transformStamped = tfBuffer.lookupTransform("world", o5g.name, ros::Time(0));
} catch (tf2::TransformException &ex) { } catch (tf2::TransformException &ex) {
ROS_WARN("%s", ex.what()); ROS_WARN("%s", ex.what());
ros::Duration(0.1).sleep(); ros::Duration(0.1).sleep();
continue; continue;
} }
geometry_msgs::PoseStamped ros_msg;
ros_msg.header.stamp = transformStamped.header.stamp;
ros_msg.header.frame_id = "joint";
//ros_msg.pose.position.x = transformStamped.transform.translation.x;
//ros_msg.pose.position.y = transformStamped.transform.translation.y;
//ros_msg.pose.position.z = transformStamped.transform.translation.z;
ros_msg.pose.orientation.x = transformStamped.transform.rotation.x;
ros_msg.pose.orientation.y = transformStamped.transform.rotation.y;
ros_msg.pose.orientation.z = transformStamped.transform.rotation.z;
//ros_msg.pose.orientation.w = transformStamped.transform.rotation.w;
//ros_msg.pose.orientation.x = transformStamped.transform.rotation.x;
//ros_msg.pose.orientation.y = transformStamped.transform.rotation.y;
//ros_msg.pose.orientation.z = transformStamped.transform.rotation.z;
ros_msg.pose.orientation.w = transformStamped.transform.rotation.w;
ros_msg.pose.orientation.x = transformStamped.transform.rotation.x;
ros_msg.pose.orientation.y = transformStamped.transform.rotation.y;
ros_msg.pose.orientation.z = transformStamped.transform.rotation.z;
//turtle_vel.publish(ros_msg);
//publishers[joint.second].publish(ros_msg);
// publish the MQTT message
robot::RobotState pls_msg;
//robot::RobotState pls_msg;
// set the name to the MQTT topic provided by the topics map
pls_msg.set_name(topic.second);
// robot::RobotState pls_msg;
auto *pos = new robot::RobotState_Position();
pos->set_o5a.p1(transformStamped.transform.translation.x);
pos->set_o5b.p1(transformStamped.transform.translation.y);
pos->set_o5c.p1(transformStamped.transform.translation.z);
pos->set_o5d.p1(transformStamped.transform.translation.z);
pos->set_o5e.p1(transformStamped.transform.translation.z);
pos->set_o5f.p1(transformStamped.transform.translation.z);
pos->set_o5g.p1(transformStamped.transform.translation.z);
pls_msg.set_allocated_position(pos);
//auto *pos = new robot::RobotState_Position();
//pos->set_x(transformStamped.transform.translation.x);
//pos->set_y(transformStamped.transform.translation.y);
//pos->set_z(transformStamped.transform.translation.z);
//pls_msg.set_allocated_position(pos);
auto *orient = new robot::RobotState_Orientation();
orient->set_o5a.or2(transformStamped.transform.rotation.w);
orient->set_o5b.or2(transformStamped.transform.rotation.x);
orient->set_o5c.or2(transformStamped.transform.rotation.y);
orient->set_o5d.or2(transformStamped.transform.rotation.z);
orient->set_o5e.or2(transformStamped.transform.rotation.z);
orient->set_o5f.or2(transformStamped.transform.rotation.z);
orient->set_o5g.or2(transformStamped.transform.rotation.z);
pls_msg.set_allocated_orientation(orient);
//auto *orient = new robot::RobotState_Orientation();
//orient->set_w(transformStamped.transform.rotation.w);
//orient->set_x(transformStamped.transform.rotation.x);
//orient->set_y(transformStamped.transform.rotation.y);
//orient->set_z(transformStamped.transform.rotation.z);
//pls_msg.set_allocated_orientation(orient);
//robot_models_node::sendMqttMessages(pls_msg);
//robot_state_provider::sendMqttMessages(pls_msg);
} }
rate.sleep(); rate.sleep();
} }
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment