Select Git revision
collision_helper.cpp
robot_models_node.cpp 8.35 KiB
#include <ros/ros.h>
#include </home/nikhil/panda_gazebo_workspace/src/zero/src/datalink/abstractobject.h>
#include </home/nikhil/panda_gazebo_workspace/src/zero/src/datalink/graspobject.h>
#include </home/nikhil/panda_gazebo_workspace/src/zero/src/datalink/hand.h>
#include </home/nikhil/panda_gazebo_workspace/src/zero/src/datalink/humanspace.h>
#include </home/nikhil/panda_gazebo_workspace/src/zero/src/datalink/joint.h>
#include </home/nikhil/panda_gazebo_workspace/src/zero/src/datalink/obstacle.h>
#include </home/nikhil/panda_gazebo_workspace/src/zero/src/datalink/robot.h>
#include </home/nikhil/panda_gazebo_workspace/src/zero/src/datalink/robotfpe.h>
#include </home/nikhil/panda_gazebo_workspace/src/zero/src/datalink/world.h>
#include </home/nikhil/panda_gazebo_workspace/src/zero/src/datalink/worldobject.h>
#include </home/nikhil/panda_gazebo_workspace/src/zero/src/datalink/quaternion.h>
#include <vector>
#include <iostream>
#include <tf2_ros/transform_listener.h>
#include <geometry_msgs/TransformStamped.h>
#include <geometry_msgs/PoseStamped.h>
#include <tf2_ros/static_transform_broadcaster.h>
#include <cstdio>
#include <tf2/LinearMath/Quaternion.h>
#include "tf2_ros/message_filter.h"
#include "message_filters/subscriber.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
#include "geometry_msgs/PointStamped.h"
#include <tf2_ros/transform_broadcaster.h>
#include <tf2/LinearMath/Quaternion.h>
int main(int argc, char** argv) {
ros::init(argc, argv, "ROSNODE1");
ros::NodeHandle node_handle("namespace_name");
ros::AsyncSpinner spinner(1);
spinner.start();
ROS_INFO("HELLO, WORLD");
abstractobject o1;
graspobject o2;
hand o3;
humanspace o4;
joint o5a;
joint o5b;
joint o5c;
joint o5d;
joint o5e;
joint o5f;
joint o5g;
obstacle o6;
robot o7;
robotfpe o8;
world o9;
worldobject o10;
o5a.name = "panda_joint1";
o5b.name = "panda_joint2";
o5c.name = "panda_joint3";
o5d.name = "panda_joint4";
o5e.name = "panda_joint5";
o5f.name = "panda_joint6";
o5g.name = "panda_joint7";
o5a.or2 = {1,5,6,7};
o5b.or2 = {2,3,5,4};
o5c.or2 = {5,1,9,6};
o5d.or2 = {6,3,8,9};
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.of = true;
o8.moving = false;
o8.grasppos = true;
o8.planninggrp = "Panda Teaching";
o3.grippos = {12, 14, 18};
o4.humannum = 1;
o2.name = "Cube";
o2.size = {5, 7, 9};
o2.p1 = {13, 14, 17};
o2.x = 2.3;
o2.y = 1.2;
o2.z = 0.2;
o2.w = 0.1;
o6.name = "Ball";
o6.size = {1, 2, 3};
o6.p1 = {12, 16, 17};
o6.x = 1.1;
o6.y = 1.4;
o6.z = 1.6;
o6.w = 1.9;
//ros::Publisher turtle_vel
//=node.advertise<geometry_msgs::Twist>("turtle2/cmd_vel", 10);
tf2_ros::Buffer tfBuffer;
tf2_ros::TransformListener tfListener(tfBuffer);
ros::Rate rate(10);
while (node_handle.ok()) {
//for (const auto &topic : ROSNODE1::topics)
{
geometry_msgs::TransformStamped transformStamped;
try {
transformStamped = tfBuffer.lookupTransform("world", o5a.name, ros::Time(0));
transformStamped = tfBuffer.lookupTransform("world", o5b.name, ros::Time(0));
transformStamped = tfBuffer.lookupTransform("world", o5c.name, ros::Time(0));
transformStamped = tfBuffer.lookupTransform("world", o5d.name, ros::Time(0));
transformStamped = tfBuffer.lookupTransform("world", o5e.name, ros::Time(0));
transformStamped = tfBuffer.lookupTransform("world", o5f.name, ros::Time(0));
transformStamped = tfBuffer.lookupTransform("world", o5g.name, ros::Time(0));
} catch (tf2::TransformException &ex) {
ROS_WARN("%s", ex.what());
ros::Duration(0.1).sleep();
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();
}
return 0;}