pavel/teleoperation
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

refactored the code, fixed config bug

Browse Source
This commit is contained in:
Pavel Lutskov
2019-02-08 16:42:48 +01:00
parent 949f657a75
commit 8861d215c5
11 changed files with 1022 additions and 1064 deletions
Download Patch File Download Diff File Expand all files Collapse all files

891
src/rviz_human.cpp
View File

@@ -4,401 +4,534 @@
#include <visualization_msgs/Marker.h>
#include <tf/transform_listener.h>
struct quarternion {
double x;
double y;
double z;
double w;
};
quarternion toQuaternion(double yaw, double pitch, double roll);
int main( int argc, char** argv )
{
ros::init(argc, argv, "rviz_human");
ros::NodeHandle n;
ros::Rate r(100);
ros::Publisher marker_pub = n.advertise<visualization_msgs::Marker>(
"visualization_marker", 10
);
ros::init(argc, argv, "rviz_human");
ros::NodeHandle n;
ros::Rate r(100);
ros::Publisher marker_pub = n.advertise<visualization_msgs::Marker>(
"visualization_marker", 15);
tf::TransformListener tll;
tf::TransformListener aruco_0;
tf::TransformListener aruco_1;
tf::TransformListener aruco_2;
int i = 0;
int walk = 0;
float l_leg_y = 0.0;
float r_leg_y = 0.0;
// suppose we already have the position of the center marker in the correct frame
// suppose we already have the position of the center marker
// in the correct frame
geometry_msgs::Point torso;
geometry_msgs::Point l_hand;
geometry_msgs::Point r_hand;
geometry_msgs::Point base_position;
float x_start = 2;
float y_start = 0;
float z_start = 1.3;
// Set human to starting postion
torso.x = -1;
torso.y = 0;
torso.z = 0;
float x_0 = 2;
float y_0 = 0;
float z_0 = 1.3;
l_hand.x = -1;
l_hand.y = 0.35;
l_hand.z = -0.2;
float x_1 = 0;
float y_1 = 0;
float z_1 = 0;
r_hand.x = -1;
r_hand.y = -0.35;
r_hand.z = -0.2;
while (ros::ok())
{
base_position.x = -1;
base_position.y = 0;
base_position.z = -1;
// tried to subscribe to tf to recieve marker coordinates
tf::StampedTransform aruco_0_tf;
tf::StampedTransform aruco_1_tf;
tf::StampedTransform aruco_2_tf;
double base_r = 2;
try {
tll.lookupTransform(
"/odom", "/Aruco_0_frame",ros::Time(0), aruco_0_tf);
/*ROS_INFO("0: %f",aruco_0_tf.getOrigin()[0]);
ROS_INFO("1: %f",aruco_0_tf.getOrigin()[1]);
ROS_INFO("2: %f",aruco_0_tf.getOrigin()[2]);
*/
tll.lookupTransform(
"/Aruco_0_frame", "/Aruco_1_frame",ros::Time(0), aruco_1_tf);
/*
ROS_INFO("0_1: %f",aruco_1_tf.getOrigin()[0]);
ROS_INFO("1_1: %f",aruco_1_tf.getOrigin()[1]);
ROS_INFO("2_1: %f",aruco_1_tf.getOrigin()[2]);
*/
x_0 = aruco_0_tf.getOrigin()[0];
y_0 = aruco_0_tf.getOrigin()[1];
z_0 = aruco_0_tf.getOrigin()[2];
x_1 = aruco_1_tf.getOrigin()[0];
y_1 = aruco_1_tf.getOrigin()[1];
z_1 = aruco_1_tf.getOrigin()[2];
}
/*
catch (tf::TransformException ex){
ROS_ERROR("%s",ex.what());
ros::Duration(1.0).sleep();
}
try{
*/
catch (tf::TransformException ex) {
ROS_ERROR("%s",ex.what());
ros::Duration(1.0).sleep();
}
visualization_msgs::Marker body;
visualization_msgs::Marker head;
visualization_msgs::Marker l_leg;
visualization_msgs::Marker r_leg;
visualization_msgs::Marker l_arm;
visualization_msgs::Marker r_arm;
visualization_msgs::Marker camera;
// Set the frame ID and timestamp.
// See the TF tutorials for information on these.
body.header.frame_id = "/odom";
body.header.stamp = ros::Time::now();
head.header.frame_id = "/odom";
head.header.stamp = ros::Time::now();
l_leg.header.frame_id = "/odom";
l_leg.header.stamp = ros::Time::now();
r_leg.header.frame_id = "/odom";
r_leg.header.stamp = ros::Time::now();
l_arm.header.frame_id = "/odom";
l_arm.header.stamp = ros::Time::now();
r_arm.header.frame_id = "/odom";
r_arm.header.stamp = ros::Time::now();
camera.header.frame_id = "/odom";
camera.header.stamp = ros::Time::now();
body.ns = "body";
body.id = 0;
head.ns = "head";
head.id = 1;
l_leg.ns = "l_leg";
l_leg.id = 2;
r_leg.ns = "r_leg";
r_leg.id = 3;
l_arm.ns = "l_arm";
l_arm.id = 4;
r_arm.ns = "r_arm";
r_arm.id = 5;
camera.ns = "r_arm";
camera.id = 6;
body.type = visualization_msgs::Marker::CUBE;
head.type = visualization_msgs::Marker::SPHERE;
l_leg.type = visualization_msgs::Marker::CYLINDER;
r_leg.type = visualization_msgs::Marker::CYLINDER;
l_arm.type = visualization_msgs::Marker::CYLINDER;
r_arm.type = visualization_msgs::Marker::CYLINDER;
camera.type = visualization_msgs::Marker::CUBE;
// Set the marker action.
// Options are ADD, DELETE, and new in ROS Indigo: 3 (DELETEALL)
body.action = visualization_msgs::Marker::ADD;
head.action = visualization_msgs::Marker::ADD;
l_arm.action = visualization_msgs::Marker::ADD;
r_arm.action = visualization_msgs::Marker::ADD;
l_leg.action = visualization_msgs::Marker::ADD;
r_arm.action = visualization_msgs::Marker::ADD;
camera.action = visualization_msgs::Marker::ADD;
// Set the pose of the marker.
// This is a full 6DOF pose relative to the
// frame/time specified in the header
body.pose.position.x = x_0;
body.pose.position.y = y_0;
body.pose.position.z = z_0;
body.pose.orientation.x = 0.0;
body.pose.orientation.y = 0.0;
body.pose.orientation.z = 0.0;
body.pose.orientation.w = 1.0;
head.pose.position.x = x_0;
head.pose.position.y = y_0;
head.pose.position.z = z_0+0.5; //1.85
head.pose.orientation.x = 0.0;
head.pose.orientation.y = 0.0;
head.pose.orientation.z = 0.0;
head.pose.orientation.w = 1.0;
l_leg.pose.position.x = x_0;
l_leg.pose.position.y = y_0+0.2;
l_leg.pose.position.z = z_0-0.8; //0.4
l_leg.pose.orientation.x = 0.0;
l_leg.pose.orientation.y = l_leg_y;
l_leg.pose.orientation.z = 0.0;
l_leg.pose.orientation.w = 1.0;
r_leg.pose.position.x = x_0;
r_leg.pose.position.y = y_0-0.2;
r_leg.pose.position.z = z_0-0.8; //0.4
r_leg.pose.orientation.x = 0.0;
r_leg.pose.orientation.y = r_leg_y;
r_leg.pose.orientation.z = 0.0;
r_leg.pose.orientation.w = 1.0;
// calculate left arm angel in z y plane; arm length is 0.5
float alpha = atan(z_1/y_1);
float delta_z = 0;//0.25*sin(alpha);
float delta_y = 0;//0.35*cos(alpha);
//rad to degree
alpha = alpha*180/3.1415;
quarternion q = toQuaternion(0,0, alpha);
ROS_INFO("x: %f, y: %f, z: %f", x_1, y_1, z_1);
ROS_INFO("alpha: %f, z: %f, y: %f", alpha, delta_z, delta_y);
ROS_INFO("qx: %f, qy: %f, qz: %f, qw: %f", q.x,q.y,q.z,q.w);
l_arm.pose.position.x = x_0;
l_arm.pose.position.y = y_0+0.4+delta_y;
l_arm.pose.position.z = z_0+0.3+delta_z; //1.6
l_arm.pose.orientation.x = 0.0+q.x;//+alpha;
l_arm.pose.orientation.y = 0.0+q.y;
l_arm.pose.orientation.z = 0.0+q.z;
l_arm.pose.orientation.w = 1.0+q.w;
r_arm.pose.position.x = x_0;
r_arm.pose.position.y = y_0-0.4;
r_arm.pose.position.z = z_0+0.3; //1.6
r_arm.pose.orientation.x = 1.0;
r_arm.pose.orientation.y = 0.0;
r_arm.pose.orientation.z = 0.0;
r_arm.pose.orientation.w = 1.0;
camera.pose.position.x = 0;
camera.pose.position.y = 0;
camera.pose.position.z = 0.5;
camera.pose.orientation.x = 0.0;
camera.pose.orientation.y = 0.0;
camera.pose.orientation.z = 0.0;
camera.pose.orientation.w = 0.0;
// Set the scale of the marker -- 1x1x1 here means 1m on a side
body.scale.x = 0.2;
body.scale.y = 0.4;
body.scale.z = 0.8;
head.scale.x = 0.3;
head.scale.y = 0.3;
head.scale.z = 0.4;
l_leg.scale.x = 0.2;
l_leg.scale.y = 0.2;
l_leg.scale.z = 0.9;
r_leg.scale.x = 0.2;
r_leg.scale.y = 0.2;
r_leg.scale.z = 0.9;
l_arm.scale.x = 0.2;
l_arm.scale.y = 0.2;
l_arm.scale.z = 0.5;
r_arm.scale.x = 0.2;
r_arm.scale.y = 0.2;
r_arm.scale.z = 0.5;
camera.scale.x = 0.5;
camera.scale.y = 0.5;
camera.scale.z = 1;
// Set the color -- be sure to set alpha to something non-zero!
body.color.r = 0.0f;
body.color.g = 1.0f;
body.color.b = 0.5f;
body.color.a = 1.0;
head.color.r = 0.0f;
head.color.g = 1.0f;
head.color.b = 0.0f;
head.color.a = 1.0;
l_leg.color.r = 1.0f;
l_leg.color.g = 0.5f;
l_leg.color.b = 0.0f;
l_leg.color.a = 1.0;
r_leg.color.r = 1.0f;
r_leg.color.g = 0.5f;
r_leg.color.b = 0.0f;
r_leg.color.a = 1.0;
l_arm.color.r = 1.0f;
l_arm.color.g = 0.5f;
l_arm.color.b = 1.0f;
l_arm.color.a = 1.0;
r_arm.color.r = 1.0f;
r_arm.color.g = 0.5f;
r_arm.color.b = 1.0f;
r_arm.color.a = 1.0;
camera.color.r = 1.0f;
camera.color.g = 1.0f;
camera.color.b = 1.0f;
camera.color.a = 1.0;
body.lifetime = ros::Duration();
head.lifetime = ros::Duration();
l_leg.lifetime = ros::Duration();
r_leg.lifetime = ros::Duration();
l_arm.lifetime = ros::Duration();
r_arm.lifetime = ros::Duration();
camera.lifetime = ros::Duration();
// Publish the marker
while (marker_pub.getNumSubscribers() < 1)
while (ros::ok())
{
if (!ros::ok())
{
return 0;
}
ROS_WARN_ONCE("Please create a subscriber to the marker");
sleep(1);
}
// tried to subscribe to tf to recieve marker coordinates
tf::StampedTransform aruco_0_tf;
tf::StampedTransform aruco_1_tf;
tf::StampedTransform aruco_2_tf;
try{
aruco_0.lookupTransform("/odom", "/Aruco_0_frame",ros::Time(0), aruco_0_tf);
ROS_INFO("0: %f",aruco_0_tf.getOrigin()[0]);
ROS_INFO("1: %f",aruco_0_tf.getOrigin()[1]);
ROS_INFO("2: %f",aruco_0_tf.getOrigin()[2]);
aruco_1.lookupTransform("/odom", "/Aruco_1_frame",ros::Time(0), aruco_1_tf);
ROS_INFO("0_1: %f",aruco_1_tf.getOrigin()[0]);
ROS_INFO("1_1: %f",aruco_1_tf.getOrigin()[1]);
ROS_INFO("2_1: %f",aruco_1_tf.getOrigin()[2]);
aruco_2.lookupTransform("/odom", "/Aruco_2_frame",ros::Time(0), aruco_2_tf);
ROS_INFO("0_2: %f",aruco_2_tf.getOrigin()[0]);
ROS_INFO("1_2: %f",aruco_2_tf.getOrigin()[1]);
ROS_INFO("2_2: %f",aruco_2_tf.getOrigin()[2]);
// update torso and hand positions
torso.x = aruco_0_tf.getOrigin()[0];
torso.y = aruco_0_tf.getOrigin()[1];
torso.z = aruco_0_tf.getOrigin()[2];
l_hand.x = aruco_1_tf.getOrigin()[0];
l_hand.y = aruco_1_tf.getOrigin()[1];
l_hand.z = aruco_1_tf.getOrigin()[2];
r_hand.x = aruco_2_tf.getOrigin()[0];
r_hand.y = aruco_2_tf.getOrigin()[1];
r_hand.z = aruco_2_tf.getOrigin()[2];
// little walking animation
i++;
if(i % 100 == 0)
{
if(walk == 0)
{
walk = 1;
l_leg_y = 0.2;
r_leg_y = -0.2;
}
else
{
l_leg_y = -0.2;
r_leg_y = 0.2;
walk = 0;
catch (tf::TransformException ex){
ROS_INFO("%s",ex.what());
ros::Duration(1.0).sleep();
}
visualization_msgs::Marker body;
visualization_msgs::Marker head;
visualization_msgs::Marker l_shoulder;
visualization_msgs::Marker r_shoulder;
visualization_msgs::Marker l_joints;
visualization_msgs::Marker r_joints;
visualization_msgs::Marker l_arm;
visualization_msgs::Marker r_arm;
visualization_msgs::Marker l_legjoints;
visualization_msgs::Marker r_legjoints;
visualization_msgs::Marker l_leg;
visualization_msgs::Marker r_leg;
visualization_msgs::Marker camera;
visualization_msgs::Marker base;
// Set the frame ID and timestamp
body.header.frame_id = "/odom";
body.header.stamp = ros::Time::now();
head.header.frame_id = "/odom";
head.header.stamp = ros::Time::now();
l_shoulder.header.frame_id = "/odom";
l_shoulder.header.stamp = ros::Time::now();
r_shoulder.header.frame_id = "/odom";
r_shoulder.header.stamp = ros::Time::now();
l_joints.header.frame_id = "/odom";
l_joints.header.stamp = ros::Time::now();
r_joints.header.frame_id = "/odom";
r_joints.header.stamp = ros::Time::now();
l_arm.header.frame_id = "/odom";
l_arm.header.stamp = ros::Time::now();
r_arm.header.frame_id = "/odom";
r_arm.header.stamp = ros::Time::now();
l_legjoints.header.frame_id = "/odom";
l_legjoints.header.stamp = ros::Time::now();
r_legjoints.header.frame_id = "/odom";
r_legjoints.header.stamp = ros::Time::now();
l_leg.header.frame_id = "/odom";
l_leg.header.stamp = ros::Time::now();
r_leg.header.frame_id = "/odom";
r_leg.header.stamp = ros::Time::now();
camera.header.frame_id = "/odom";
camera.header.stamp = ros::Time::now();
base.header.frame_id = "/odom";
base.header.stamp = ros::Time::now();
// set namespace and id
body.ns = "body";
body.id = 0;
head.ns = "head";
head.id = 1;
l_shoulder.ns = "l_shoulder";
l_shoulder.id = 2;
r_shoulder.ns = "r_shoulder";
r_shoulder.id = 3;
l_joints.ns = "l_joints";
l_joints.id = 4;
r_joints.ns = "l_joints";
r_joints.id = 5;
l_arm.ns = "l_arm";
l_arm.id = 6;
r_arm.ns = "r_arm";
r_arm.id = 7;
l_legjoints.ns = "l_legjoints";
l_legjoints.id = 8;
r_legjoints.ns = "l_legjoints";
r_legjoints.id = 9;
l_leg.ns = "l_leg";
l_leg.id = 10;
r_leg.ns = "r_leg";
r_leg.id = 11;
camera.ns = "camera";
camera.id = 12;
base.ns = "base";
base.id = 13;
// set marker shape
body.type = visualization_msgs::Marker::CUBE;
head.type = visualization_msgs::Marker::SPHERE;
l_shoulder.type = visualization_msgs::Marker::CUBE;
r_shoulder.type = visualization_msgs::Marker::CUBE;
l_joints.type = visualization_msgs::Marker::POINTS;
r_joints.type = visualization_msgs::Marker::POINTS;
l_arm.type = visualization_msgs::Marker::LINE_STRIP;
r_arm.type = visualization_msgs::Marker::LINE_STRIP;
l_legjoints.type = visualization_msgs::Marker::POINTS;
r_legjoints.type = visualization_msgs::Marker::POINTS;
l_leg.type = visualization_msgs::Marker::LINE_STRIP;
r_leg.type = visualization_msgs::Marker::LINE_STRIP;
camera.type = visualization_msgs::Marker::SPHERE;
base.type = visualization_msgs::Marker::SPHERE;
// Set the marker action
body.action = visualization_msgs::Marker::ADD;
head.action = visualization_msgs::Marker::ADD;
l_shoulder.action = visualization_msgs::Marker::ADD;
r_shoulder.action = visualization_msgs::Marker::ADD;
l_joints.action = visualization_msgs::Marker::ADD;
r_joints.action = visualization_msgs::Marker::ADD;
l_arm.action = visualization_msgs::Marker::ADD;
r_arm.action = visualization_msgs::Marker::ADD;
l_legjoints.action = visualization_msgs::Marker::ADD;
r_legjoints.action = visualization_msgs::Marker::ADD;
l_leg.action = visualization_msgs::Marker::ADD;
r_leg.action = visualization_msgs::Marker::ADD;
camera.action = visualization_msgs::Marker::ADD;
base.action = visualization_msgs::Marker::ADD;
// Set the pose of the marker
body.pose.position.x = torso.x;
body.pose.position.y = torso.y;
body.pose.position.z = torso.z;
body.pose.orientation.x = 0.0;
body.pose.orientation.y = 0.0;
body.pose.orientation.z = 0.0;
body.pose.orientation.w = 1.0;
head.pose.position.x = torso.x;
head.pose.position.y = torso.y;
head.pose.position.z = torso.z+0.5;
head.pose.orientation.x = 0.0;
head.pose.orientation.y = 0.0;
head.pose.orientation.z = 0.0;
head.pose.orientation.w = 1.0;
l_shoulder.pose.position.x = torso.x;
l_shoulder.pose.position.y = torso.y+0.3;
l_shoulder.pose.position.z = torso.z+0.3;
l_shoulder.pose.orientation.x = 0.0;
l_shoulder.pose.orientation.y = 0.0;
l_shoulder.pose.orientation.z = 0.0;
l_shoulder.pose.orientation.w = 1.0;
r_shoulder.pose.position.x = torso.x;
r_shoulder.pose.position.y = torso.y-0.3;
r_shoulder.pose.position.z = torso.z+0.3;
r_shoulder.pose.orientation.x = 0.0;
r_shoulder.pose.orientation.y = 0.0;
r_shoulder.pose.orientation.z = 0.0;
r_shoulder.pose.orientation.w = 1.0;
l_joints.pose.orientation.w = 1.0;
r_joints.pose.orientation.w = 1.0;
l_arm.pose.orientation.w = 1.0;
r_arm.pose.orientation.w = 1.0;
l_legjoints.pose.orientation.w = 1.0;
r_legjoints.pose.orientation.w = 1.0;
l_leg.pose.orientation.w = 1.0;
r_leg.pose.orientation.w = 1.0;
camera.pose.position.x = 0;
camera.pose.position.y = 0;
camera.pose.position.z = 0.0;
camera.pose.orientation.x = 0.0;
camera.pose.orientation.y = 0.0;
camera.pose.orientation.z = 0.0;
camera.pose.orientation.w = 0.0;
base.pose.position.x = base_position.x;
base.pose.position.y = base_position.y;
base.pose.position.z = base_position.z;
base.pose.orientation.x = 0.0;
base.pose.orientation.y = 0.0;
base.pose.orientation.z = 0.0;
base.pose.orientation.w = 1.0;
// Set the scale of the marker (in meters)
body.scale.x = 0.2;
body.scale.y = 0.4;
body.scale.z = 0.8;
head.scale.x = 0.3;
head.scale.y = 0.3;
head.scale.z = 0.4;
l_leg.scale.x = 0.2;
l_leg.scale.y = 0.2;
l_leg.scale.z = 0.9;
r_leg.scale.x = 0.2;
r_leg.scale.y = 0.2;
r_leg.scale.z = 0.9;
l_shoulder.scale.x = 0.2;
l_shoulder.scale.y = 0.2;
l_shoulder.scale.z = 0.2;
r_shoulder.scale.x = 0.2;
r_shoulder.scale.y = 0.2;
r_shoulder.scale.z = 0.2;
l_joints.scale.x = 0.2;
l_joints.scale.y = 0.2;
r_joints.scale.x = 0.2;
r_joints.scale.y = 0.2;
l_arm.scale.x = 0.1;
l_arm.scale.y = 0.1;
r_arm.scale.x = 0.1;
r_arm.scale.y = 0.1;
l_legjoints.scale.x = 0.2;
l_legjoints.scale.y = 0.2;
r_legjoints.scale.x = 0.2;
r_legjoints.scale.y = 0.2;
l_leg.scale.x = 0.2;
l_leg.scale.y = 0.2;
r_leg.scale.x = 0.2;
r_leg.scale.y = 0.2;
camera.scale.x = 0.5;
camera.scale.y = 0.5;
camera.scale.z = 0.5;
base.scale.x = base_r;
base.scale.y = base_r;
base.scale.z = 0.01;
// Set the color
body.color.r = 0.0f;
body.color.g = 1.0f;
body.color.b = 0.5f;
body.color.a = 1.0;
head.color.r = 0.0f;
head.color.g = 1.0f;
head.color.b = 0.0f;
head.color.a = 1.0;
l_shoulder.color.r = 0.5f;
l_shoulder.color.g = 0.2f;
l_shoulder.color.b = 1.0f;
l_shoulder.color.a = 1.0;
r_shoulder.color.r = 0.5f;
r_shoulder.color.g = 0.2f;
r_shoulder.color.b = 1.0f;
r_shoulder.color.a = 1.0;
l_joints.color.r = 0.5f;
l_joints.color.g = 0.2f;
l_joints.color.b = 1.0f;
l_joints.color.a = 1.0;
r_joints.color.r = 0.5f;
r_joints.color.g = 0.2f;
r_joints.color.b = 1.0f;
r_joints.color.a = 1.0;
l_arm.color.r = 0.5f;
l_arm.color.g = 0.2f;
l_arm.color.b = 1.0f;
l_arm.color.a = 1.0;
r_arm.color.r = 0.5f;
r_arm.color.g = 0.2f;
r_arm.color.b = 1.0f;
r_arm.color.a = 1.0;
l_legjoints.color.r = 0.5f;
l_legjoints.color.g = 0.2f;
l_legjoints.color.b = 1.0f;
l_legjoints.color.a = 1.0;
r_legjoints.color.r = 0.5f;
r_legjoints.color.g = 0.2f;
r_legjoints.color.b = 1.0f;
r_legjoints.color.a = 1.0;
l_leg.color.r = 0.5f;
l_leg.color.g = 0.2f;
l_leg.color.b = 1.0f;
l_leg.color.a = 1.0;
r_leg.color.r = 0.5f;
r_leg.color.g = 0.2f;
r_leg.color.b = 1.0f;
r_leg.color.a = 1.0;
camera.color.r = 1.0f;
camera.color.g = 1.0f;
camera.color.b = 1.0f;
camera.color.a = 1.0;
base.color.r = 1.0f;
base.color.g = 0.0f;
base.color.b = 0.0f;
base.color.a = 1.0;
// set lifetime
body.lifetime = ros::Duration();
head.lifetime = ros::Duration();
l_shoulder.lifetime = ros::Duration();
r_shoulder.lifetime = ros::Duration();
l_joints.lifetime = ros::Duration();
r_joints.lifetime = ros::Duration();
l_arm.lifetime = ros::Duration();
r_arm.lifetime = ros::Duration();
l_legjoints.lifetime = ros::Duration();
r_legjoints.lifetime = ros::Duration();
l_leg.lifetime = ros::Duration();
r_leg.lifetime = ros::Duration();
camera.lifetime = ros::Duration();
base.lifetime = ros::Duration();
// create lines
// create line for arms
geometry_msgs::Point l_shoulder_point;
geometry_msgs::Point r_shoulder_point;
l_shoulder_point.x = torso.x;
l_shoulder_point.y = torso.y+0.3;
l_shoulder_point.z = torso.z+0.3;
r_shoulder_point.x = torso.x;
r_shoulder_point.y = torso.y-0.3;
r_shoulder_point.z = torso.z+0.3;
l_joints.points.push_back(l_shoulder_point);
l_joints.points.push_back(l_hand);
l_arm.points.push_back(l_shoulder_point);
l_arm.points.push_back(l_hand);
r_joints.points.push_back(r_shoulder_point);
r_joints.points.push_back(r_hand);
r_arm.points.push_back(r_shoulder_point);
r_arm.points.push_back(r_hand);
// create line for legs
geometry_msgs::Point l_hip;
geometry_msgs::Point r_hip;
geometry_msgs::Point l_knee;
geometry_msgs::Point r_knee;
geometry_msgs::Point l_foot;
geometry_msgs::Point r_foot;
l_hip.x = torso.x;
l_hip.y = torso.y+0.2;
l_hip.z = torso.z-0.15;
r_hip.x = torso.x;
r_hip.y = torso.y-0.2;
r_hip.z = torso.z-0.15;
l_knee.x = torso.x;
l_knee.y = torso.y+0.2;
l_knee.z = torso.z-0.55;
r_knee.x = torso.x;
r_knee.y = torso.y-0.2;
r_knee.z = torso.z-0.55;
l_foot.x = torso.x;
l_foot.y = torso.y+0.2;
l_foot.z = torso.z-0.95;
r_foot.x = torso.x;
r_foot.y = torso.y-0.2;
r_foot.z = torso.z-0.95;
// push to points and lines
l_legjoints.points.push_back(l_hip);
l_legjoints.points.push_back(l_knee);
l_legjoints.points.push_back(l_foot);
l_leg.points.push_back(l_hip);
l_leg.points.push_back(l_knee);
l_leg.points.push_back(l_foot);
r_legjoints.points.push_back(r_hip);
r_legjoints.points.push_back(r_knee);
r_legjoints.points.push_back(r_foot);
r_leg.points.push_back(r_hip);
r_leg.points.push_back(r_knee);
r_leg.points.push_back(r_foot);
// Publish the markers
while (marker_pub.getNumSubscribers() < 1)
{
if (!ros::ok())
{
return 0;
}
ROS_WARN_ONCE("Please create a subscriber to the marker");
sleep(1);
}
marker_pub.publish(body);
marker_pub.publish(head);
marker_pub.publish(l_shoulder);
marker_pub.publish(r_shoulder);
marker_pub.publish(l_joints);
marker_pub.publish(r_joints);
marker_pub.publish(l_arm);
marker_pub.publish(r_arm);
marker_pub.publish(l_legjoints);
marker_pub.publish(r_legjoints);
marker_pub.publish(l_leg);
marker_pub.publish(r_leg);
marker_pub.publish(camera);
marker_pub.publish(base);
r.sleep();
}
// move hole human
i++;
// publish markers
body.action = visualization_msgs::Marker::ADD;
head.action = visualization_msgs::Marker::ADD;
l_arm.action = visualization_msgs::Marker::ADD;
r_arm.action = visualization_msgs::Marker::ADD;
l_leg.action = visualization_msgs::Marker::ADD;
r_arm.action = visualization_msgs::Marker::ADD;
marker_pub.publish(body);
marker_pub.publish(head);
marker_pub.publish(l_leg);
marker_pub.publish(r_leg);
marker_pub.publish(l_arm);
marker_pub.publish(r_arm);
marker_pub.publish(camera);
i = i +1;
r.sleep();
}
}
quarternion toQuaternion( double yaw, double pitch, double roll) {
// Abbreviations for the various angular functions
double cy = cos(yaw * 0.5);
double sy = sin(yaw * 0.5);
double cp = cos(pitch * 0.5);
double sp = sin(pitch * 0.5);
double cr = cos(roll * 0.5);
double sr = sin(roll * 0.5);
quarternion q;
q.x = cy * cp * sr - sy * sp * cr;
q.y = sy * cp * sr + cy * sp * cr;
q.z = sy * cp * cr - cy * sp * sr;
q.w = cy * cp * cr + sy * sp * sr;
return q;
}