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

Merge remote-tracking branch 'origin/master'

Browse Source
This commit is contained in:
Pavel Lutskov
2019-03-01 10:48:34 +01:00
parent 30986d1bc5 ac6f7433e6
commit 9ad2130096
1 changed files with 44 additions and 48 deletions
Download Patch File Download Diff File Expand all files Collapse all files

90
src/rviz_human.cpp
View File

@@ -1,3 +1,17 @@
/**
* Publishes model of human,movement zone and camera consisting of markers to rviz.
*
* A YAML config file is opended to get the starting postion of the model
* and to define the size of the movement zone.
* Tf is subscribed to get the positions of the aruco markers on the
* operator. Using those inputs a model is published to rivz in each
* iteration.
*
* default.yaml: file contains points of the operator and the size
* of the action zone which are determined in a calibration
* step.
*/
#include <iostream> #include <iostream>
#include <math.h> #include <math.h>
#include <ros/ros.h> #include <ros/ros.h>
@@ -14,36 +28,26 @@ int main( int argc, char** argv )
ros::Publisher marker_pub = n.advertise<visualization_msgs::Marker>( ros::Publisher marker_pub = n.advertise<visualization_msgs::Marker>(
"visualization_marker", 15); "visualization_marker", 15);
YAML::Node config = YAML::LoadFile("/home/hrs_e/catkin_ws/src/teleoperation/config/default.yaml");
ROS_INFO("%lf", config["arm"].as<double>());
tf::TransformListener aruco_0; tf::TransformListener aruco_0;
tf::TransformListener aruco_1; tf::TransformListener aruco_1;
tf::TransformListener aruco_2; tf::TransformListener aruco_2;
// suppose we already have the position of the center marker
// in the correct frame
geometry_msgs::Point torso; geometry_msgs::Point torso;
geometry_msgs::Point l_hand; geometry_msgs::Point l_hand;
geometry_msgs::Point r_hand; geometry_msgs::Point r_hand;
geometry_msgs::Point base_position_in; geometry_msgs::Point base_position_in;
geometry_msgs::Point base_position_out; geometry_msgs::Point base_position_out;
/* Open YAML config file for starting position */
YAML::Node config = YAML::LoadFile("/home/hrs_e/catkin_ws/src/teleoperation/config/default.yaml");
// !!! YAML PARSING WORKS LIKE THIS !!! /* Set human-model to starting postion (independent of camera input) */
// YAML::Node config = YAML::LoadFile(
// "/home/hrs_e/catkin_ws/src/teleoperation/config/default.yaml");
// ROS_INFO("%lf", config["arm"].as<double>());
// Set human to starting postion
std::vector<double> center = config["cr"].as<std::vector<double> >(); std::vector<double> center = config["cr"].as<std::vector<double> >();
std::vector<double> l_sh = config["lsh"].as<std::vector<double> >(); std::vector<double> l_sh = config["lsh"].as<std::vector<double> >();
std::vector<double> r_sh = config["rsh"].as<std::vector<double> >(); std::vector<double> r_sh = config["rsh"].as<std::vector<double> >();
double size = config["arm"].as<double>()*2; /* approximate size of human as 2.2 times the arm lenth */
double size = config["arm"].as<double>()*2.2;
torso.x = center[0]; torso.x = center[0];
torso.y = center[1]; torso.y = center[1];
@@ -65,18 +69,16 @@ int main( int argc, char** argv )
base_position_out.y = center[1]; base_position_out.y = center[1];
base_position_out.z = torso.z-0.75*size; base_position_out.z = torso.z-0.75*size;
/* define radius of inner and outer circle of movement zone */
double base_in_r_x = (config["fw"].as<double>() - center[0])*0.33; double base_in_r_x = (config["fw"].as<double>() - center[0])*0.33;
double base_out_r_x = config["fw"].as<double>() - center[0]; double base_out_r_x = config["fw"].as<double>() - center[0];
double base_in_r_y = (config["rt"].as<double>() - center[1])*0.33; double base_in_r_y = (config["rt"].as<double>() - center[1])*0.33;
double base_out_r_y = (config["rt"].as<double>() - center[1]); double base_out_r_y = (config["rt"].as<double>() - center[1]);
while (ros::ok()) while (ros::ok())
{ {
/* Subscribe to tf to recieve marker coordinates */
// tried to subscribe to tf to recieve marker coordinates
tf::StampedTransform aruco_0_tf; tf::StampedTransform aruco_0_tf;
tf::StampedTransform aruco_1_tf; tf::StampedTransform aruco_1_tf;
tf::StampedTransform aruco_2_tf; tf::StampedTransform aruco_2_tf;
@@ -100,7 +102,7 @@ int main( int argc, char** argv )
ROS_INFO("1_2: %f",aruco_2_tf.getOrigin()[1]); ROS_INFO("1_2: %f",aruco_2_tf.getOrigin()[1]);
ROS_INFO("2_2: %f",aruco_2_tf.getOrigin()[2]); ROS_INFO("2_2: %f",aruco_2_tf.getOrigin()[2]);
// update torso and hand positions /* Update positions of torso and hands */
torso.x = aruco_0_tf.getOrigin()[0]; torso.x = aruco_0_tf.getOrigin()[0];
torso.y = aruco_0_tf.getOrigin()[1]; torso.y = aruco_0_tf.getOrigin()[1];
torso.z = aruco_0_tf.getOrigin()[2]; torso.z = aruco_0_tf.getOrigin()[2];
@@ -119,6 +121,7 @@ int main( int argc, char** argv )
ros::Duration(1.0).sleep(); ros::Duration(1.0).sleep();
} }
/* Create markers */
visualization_msgs::Marker body; visualization_msgs::Marker body;
visualization_msgs::Marker head; visualization_msgs::Marker head;
visualization_msgs::Marker l_shoulder; visualization_msgs::Marker l_shoulder;
@@ -135,8 +138,7 @@ int main( int argc, char** argv )
visualization_msgs::Marker base_in; visualization_msgs::Marker base_in;
visualization_msgs::Marker base_out; visualization_msgs::Marker base_out;
// Set the frame ID and timestamp /* Set the frame ID and timestamp */
body.header.frame_id = "/odom"; body.header.frame_id = "/odom";
body.header.stamp = ros::Time::now(); body.header.stamp = ros::Time::now();
@@ -183,8 +185,7 @@ int main( int argc, char** argv )
base_out.header.stamp = ros::Time::now(); base_out.header.stamp = ros::Time::now();
// set namespace and id /* Set namespace and id */
body.ns = "body"; body.ns = "body";
body.id = 0; body.id = 0;
@@ -230,8 +231,7 @@ int main( int argc, char** argv )
base_out.ns = "base_out"; base_out.ns = "base_out";
base_out.id = 14; base_out.id = 14;
// set marker shape /* Set marker shape */
body.type = visualization_msgs::Marker::CUBE; body.type = visualization_msgs::Marker::CUBE;
head.type = visualization_msgs::Marker::SPHERE; head.type = visualization_msgs::Marker::SPHERE;
l_shoulder.type = visualization_msgs::Marker::CUBE; l_shoulder.type = visualization_msgs::Marker::CUBE;
@@ -248,8 +248,7 @@ int main( int argc, char** argv )
base_in.type = visualization_msgs::Marker::SPHERE; base_in.type = visualization_msgs::Marker::SPHERE;
base_out.type = visualization_msgs::Marker::SPHERE; base_out.type = visualization_msgs::Marker::SPHERE;
// Set the marker action /* Set the marker action */
body.action = visualization_msgs::Marker::ADD; body.action = visualization_msgs::Marker::ADD;
head.action = visualization_msgs::Marker::ADD; head.action = visualization_msgs::Marker::ADD;
l_shoulder.action = visualization_msgs::Marker::ADD; l_shoulder.action = visualization_msgs::Marker::ADD;
@@ -266,8 +265,7 @@ int main( int argc, char** argv )
base_in.action = visualization_msgs::Marker::ADD; base_in.action = visualization_msgs::Marker::ADD;
base_out.action = visualization_msgs::Marker::ADD; base_out.action = visualization_msgs::Marker::ADD;
// Set the pose of the marker /* Set the pose of the marker */
body.pose.position.x = torso.x; body.pose.position.x = torso.x;
body.pose.position.y = torso.y; body.pose.position.y = torso.y;
body.pose.position.z = torso.z-0.05*size; body.pose.position.z = torso.z-0.05*size;
@@ -334,15 +332,14 @@ int main( int argc, char** argv )
base_out.pose.orientation.z = 0.0; base_out.pose.orientation.z = 0.0;
base_out.pose.orientation.w = 1.0; base_out.pose.orientation.w = 1.0;
// Set the scale of the marker (in meters) /* Set the scale of the marker (in meters) */
body.scale.x = 0.125*size; body.scale.x = 0.125*size;
body.scale.y = 0.3*size; body.scale.y = 0.3*size;
body.scale.z = 0.4*size; body.scale.z = 0.4*size;
head.scale.x = 0.125*size; head.scale.x = 0.14*size;
head.scale.y = 0.125*size; head.scale.y = 0.14*size;
head.scale.z = 0.15*size; head.scale.z = 0.16*size;
l_shoulder.scale.x = 0.125; l_shoulder.scale.x = 0.125;
l_shoulder.scale.y = 0.125; l_shoulder.scale.y = 0.125;
@@ -388,8 +385,7 @@ int main( int argc, char** argv )
base_out.scale.y = 2*base_out_r_y; base_out.scale.y = 2*base_out_r_y;
base_out.scale.z = 0.01; base_out.scale.z = 0.01;
// Set the color /* Set the color */
body.color.r = 0.0f; body.color.r = 0.0f;
body.color.g = 1.0f; body.color.g = 1.0f;
body.color.b = 0.5f; body.color.b = 0.5f;
@@ -465,8 +461,7 @@ int main( int argc, char** argv )
base_out.color.b = 0.5f; base_out.color.b = 0.5f;
base_out.color.a = 1.0; base_out.color.a = 1.0;
// set lifetime /* Set lifetime */
body.lifetime = ros::Duration(); body.lifetime = ros::Duration();
head.lifetime = ros::Duration(); head.lifetime = ros::Duration();
l_shoulder.lifetime = ros::Duration(); l_shoulder.lifetime = ros::Duration();
@@ -483,8 +478,8 @@ int main( int argc, char** argv )
base_in.lifetime = ros::Duration(); base_in.lifetime = ros::Duration();
base_out.lifetime = ros::Duration(); base_out.lifetime = ros::Duration();
// create lines /* Create lines for arms and legs */
// create line for arms /* Lines for arms */
geometry_msgs::Point l_shoulder_point; geometry_msgs::Point l_shoulder_point;
geometry_msgs::Point r_shoulder_point; geometry_msgs::Point r_shoulder_point;
@@ -497,6 +492,7 @@ int main( int argc, char** argv )
r_shoulder_point.y = torso.y+r_sh[1]; r_shoulder_point.y = torso.y+r_sh[1];
r_shoulder_point.z = torso.z+r_sh[2]; r_shoulder_point.z = torso.z+r_sh[2];
/* Push arm joint-points to lines */
l_joints.points.push_back(l_shoulder_point); l_joints.points.push_back(l_shoulder_point);
l_joints.points.push_back(l_hand); l_joints.points.push_back(l_hand);
l_arm.points.push_back(l_shoulder_point); l_arm.points.push_back(l_shoulder_point);
@@ -507,7 +503,7 @@ int main( int argc, char** argv )
r_arm.points.push_back(r_shoulder_point); r_arm.points.push_back(r_shoulder_point);
r_arm.points.push_back(r_hand); r_arm.points.push_back(r_hand);
// create line for legs /* Lines for legs */
geometry_msgs::Point l_hip; geometry_msgs::Point l_hip;
geometry_msgs::Point r_hip; geometry_msgs::Point r_hip;
@@ -540,8 +536,7 @@ int main( int argc, char** argv )
r_foot.y = torso.y-0.1; r_foot.y = torso.y-0.1;
r_foot.z = torso.z-0.70*size; r_foot.z = torso.z-0.70*size;
// push to points and lines /* Push leg joint-points to lines */
l_legjoints.points.push_back(l_hip); l_legjoints.points.push_back(l_hip);
l_legjoints.points.push_back(l_knee); l_legjoints.points.push_back(l_knee);
l_legjoints.points.push_back(l_foot); l_legjoints.points.push_back(l_foot);
@@ -559,8 +554,7 @@ int main( int argc, char** argv )
r_leg.points.push_back(r_foot); r_leg.points.push_back(r_foot);
// Publish the markers /* Publish the markers */
while (marker_pub.getNumSubscribers() < 1) while (marker_pub.getNumSubscribers() < 1)
{ {
if (!ros::ok()) if (!ros::ok())
@@ -570,7 +564,7 @@ int main( int argc, char** argv )
ROS_WARN_ONCE("Please create a subscriber to the marker"); ROS_WARN_ONCE("Please create a subscriber to the marker");
sleep(1); sleep(1);
} }
/* Human model markers */
marker_pub.publish(body); marker_pub.publish(body);
marker_pub.publish(head); marker_pub.publish(head);
marker_pub.publish(l_shoulder); marker_pub.publish(l_shoulder);
@@ -583,7 +577,9 @@ int main( int argc, char** argv )
marker_pub.publish(r_legjoints); marker_pub.publish(r_legjoints);
marker_pub.publish(l_leg); marker_pub.publish(l_leg);
marker_pub.publish(r_leg); marker_pub.publish(r_leg);
/* Camera marker */
marker_pub.publish(camera); marker_pub.publish(camera);
/* Movement zone markers */
marker_pub.publish(base_in); marker_pub.publish(base_in);
marker_pub.publish(base_out); marker_pub.publish(base_out);