27 #ifndef _atlascar_transforms_CPP_
28 #define _atlascar_transforms_CPP_
38 #include <boost/algorithm/string.hpp>
41 #include <sensor_msgs/JointState.h>
42 #include <tf/transform_broadcaster.h>
43 #include <visualization_msgs/Marker.h>
44 #include <visualization_msgs/MarkerArray.h>
45 #include <tf/transform_listener.h>
46 #include <pcl_ros/transforms.h>
47 #include <pcl/conversions.h>
48 #include <pcl/point_cloud.h>
49 #include <pcl/point_types.h>
50 #include <interactive_markers/interactive_marker_server.h>
51 #include <interactive_markers/menu_handler.h>
52 #include <ros/package.h>
53 #include <libxml/encoding.h>
54 #include <libxml/xmlwriter.h>
55 #include <libxml/tree.h>
56 #include <libxml/parser.h>
57 #include <libxml/xpath.h>
58 #include <libxml/xpathInternals.h>
59 #include <libxml/xmlreader.h>
62 #define PFLN {printf("FILE %s LINE %d\n",__FILE__, __LINE__);}
66 using namespace boost;
67 using namespace visualization_msgs;
70 boost::shared_ptr<interactive_markers::InteractiveMarkerServer>
server;
79 std::vector<boost::shared_ptr <t_reference_frame> >
v_rf;
84 for (
size_t i=0; i<
v_rf->size(); ++i)
86 if (name == (*
v_rf)[i]->name)
100 marker.type = Marker::SPHERE;
101 marker.scale.x = msg.scale * 0.05;
102 marker.scale.y = msg.scale * 0.05;
103 marker.scale.z = msg.scale * 0.05;
105 marker.color.g = 0.7;
107 marker.color.a = 0.0;
111 void processFeedback(
const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback )
113 switch ( feedback->event_type )
115 case visualization_msgs::InteractiveMarkerFeedback::POSE_UPDATE:
116 printf(
"you are moving marker %s\n",feedback->marker_name.c_str());
121 printf(
"ret=%d\n",ret);
123 v_rf[ret]->x = feedback->pose.position.x;
124 v_rf[ret]->y = feedback->pose.position.y;
125 v_rf[ret]->z = feedback->pose.position.z;
134 InteractiveMarkerControl control;
135 control.always_visible =
true;
136 control.markers.push_back(
makeBox(msg) );
137 msg.controls.push_back( control );
138 return msg.controls.back();
141 void set_label_pos(
double x,
double y,
double z, visualization_msgs::Marker* msg, std::vector<visualization_msgs::Marker>* marker_vec)
162 geometry_msgs::Point pt;
163 visualization_msgs::Marker lmsg;
164 lmsg.pose.orientation.w = 1.0;
165 lmsg.header.stamp = ros::Time::now();
167 lmsg.scale.x = 0.02; lmsg.scale.y = 1; lmsg.scale.z = 0.095;
168 lmsg.color.r = 0.0; lmsg.color.g = 0; lmsg.color.b = 0.0; lmsg.color.a = 0.4;
169 lmsg.pose.position.x = 0; lmsg.pose.position.y = 0; lmsg.pose.position.z = 0;
170 lmsg.action = visualization_msgs::Marker::ADD;
171 lmsg.type = visualization_msgs::Marker::LINE_STRIP;
173 lmsg.header.frame_id = msg->header.frame_id;
174 lmsg.ns =
"CNT " + msg->header.frame_id;
176 pt.x=0; pt.y=0; pt.z=0;
177 lmsg.points.push_back(pt);
179 pt.x=x; pt.y=y; pt.z=z;
180 lmsg.points.push_back(pt);
181 marker_vec->push_back(lmsg);
185 msg->pose.position.x = x; msg->pose.position.y = y; msg->pose.position.z = z;
193 geometry_msgs::Point p;
194 std_msgs::ColorRGBA color;
197 visualization_msgs::Marker msg;
198 msg.pose.orientation.w = 1.0;
199 msg.header.stamp = ros::Time::now();
201 msg.scale.x = 1; msg.scale.y = 1; msg.scale.z = 0.1;
202 msg.color.r = 0.0; msg.color.g = 0; msg.color.b = 0.3; msg.color.a = 1;
203 msg.pose.position.x = 0; msg.pose.position.y = 0; msg.pose.position.z = -0.1;
204 msg.action = visualization_msgs::Marker::ADD;
205 msg.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
207 for (
size_t i=0; i<
v_rf.size(); ++i)
209 msg.header.frame_id =
v_rf[i]->name;
210 msg.ns = msg.header.frame_id.c_str();
211 msg.text = msg.header.frame_id.c_str();
213 marker_vec->push_back(msg);
219 void make6DofMarker(
bool fixed, std::string name ,
double x,
double y,
double z)
221 InteractiveMarker int_marker;
222 int_marker.header.frame_id = name;
223 int_marker.pose.position.x = x;
224 int_marker.pose.position.y = y;
225 int_marker.pose.position.z = z;
226 int_marker.scale = 0.1;
228 int_marker.name = name;
229 int_marker.description =
"";
233 InteractiveMarkerControl control;
235 control.orientation.w = 1;
236 control.orientation.x = 1;
237 control.orientation.y = 0;
238 control.orientation.z = 0;
239 control.name =
"move_x";
240 control.interaction_mode = InteractiveMarkerControl::MOVE_AXIS;
241 int_marker.controls.push_back(control);
243 control.orientation.w = 1;
244 control.orientation.x = 0;
245 control.orientation.y = 1;
246 control.orientation.z = 0;
247 control.name =
"move_z";
248 control.interaction_mode = InteractiveMarkerControl::MOVE_AXIS;
249 int_marker.controls.push_back(control);
251 control.orientation.w = 1;
252 control.orientation.x = 0;
253 control.orientation.y = 0;
254 control.orientation.z = 1;
255 control.name =
"move_y";
256 control.interaction_mode = InteractiveMarkerControl::MOVE_AXIS;
257 int_marker.controls.push_back(control);
259 server->insert(int_marker);
270 void GetName(xmlTextReaderPtr reader,
string& str)
272 xmlChar*name=xmlTextReaderName(reader);
287 xmlChar*value=xmlTextReaderValue(reader);
301 void GetAttribute(xmlTextReaderPtr reader,
const xmlChar*name,
string& str)
303 xmlChar*attribute=xmlTextReaderGetAttribute(reader,name);
304 if(!attribute)
return;
305 str=(
char*)attribute;
317 if(name==
"LINK" && xmlTextReaderNodeType(reader)==1)
322 rf->x = 0; rf->y =0; rf->z =0;
328 int main(
int argc,
char **argv)
330 ros::init(argc, argv,
"atlascar_transforms");
332 ros::Rate loop_rate(1);
333 ros::Publisher vis_pub = n.advertise<visualization_msgs::MarkerArray>(
"/axis_markers", 0 );
334 tf::TransformListener listener(n,ros::Duration(10));
336 server.reset(
new interactive_markers::InteractiveMarkerServer(
"show_axes",
"",
false) );
337 ros::Duration(0.1).sleep();
344 std::string urdf_file = ros::package::getPath(
"coordinate_frames") +
"/urdf/urdf.xml";
346 printf(
"reading %s\n",urdf_file.c_str());
349 xmlTextReaderPtr reader = xmlNewTextReaderFilename(urdf_file.c_str());
354 ret=xmlTextReaderRead(reader);
358 xmlFreeTextReader(reader);
361 ROS_ERROR(
"Failed to parse file");
366 ROS_ERROR(
"Failed to parse file");
381 visualization_msgs::MarkerArray marker_msg;
385 vis_pub.publish(marker_msg);
int main(int argc, char **argv)
std::vector< boost::shared_ptr< t_reference_frame > > v_rf
void GetValue(xmlTextReaderPtr reader, string &str)
Get the value of a xml element.
tf::TransformListener * p_listener
void set_label_pos(double x, double y, double z, visualization_msgs::Marker *msg, std::vector< visualization_msgs::Marker > *marker_vec)
int add_to_viz_markers_vec(std::vector< visualization_msgs::Marker > *marker_vec)
boost::shared_ptr< interactive_markers::InteractiveMarkerServer > server
void GetName(xmlTextReaderPtr reader, string &str)
Get the name of a xml element.
void NodeHandler(xmlTextReaderPtr reader)
int get_i_from_name(std::vector< boost::shared_ptr< t_reference_frame > > *v_rf, std::string name)
InteractiveMarkerControl & makeBoxControl(InteractiveMarker &msg)
void make6DofMarker(bool fixed, std::string name, double x, double y, double z)
void processFeedback(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
void GetAttribute(xmlTextReaderPtr reader, const xmlChar *name, string &str)
Get the value of an attribute in a xml element.
Marker makeBox(InteractiveMarker &msg)