Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[14717] pkg/trunk
|
From: <jfa...@us...> - 2009-05-01 04:47:54
|
Revision: 14717
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=14717&view=rev
Author: jfaustwg
Date: 2009-05-01 04:47:46 +0000 (Fri, 01 May 2009)
Log Message:
-----------
Rename and refactor visualization markers:
* VisualizationMarker->Marker
* x/y/z/yaw/pitch/roll -> Pose pose
* alpha/r/g/b -> ColorRGBA color
* xScale/yScale/zScale -> Vector3 scale
* Added ns field (namespace)
* Added lifetime field
* visualizationMarker topic -> visualization_marker
Modified Paths:
--------------
pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/head_pan_tilt_controller.h
pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/head_servoing_controller.h
pkg/trunk/controllers/pr2_mechanism_controllers/src/head_pan_tilt_controller.cpp
pkg/trunk/controllers/pr2_mechanism_controllers/src/head_servoing_controller.cpp
pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/endeffector_constraint_controller.h
pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/plug_controller.h
pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_hybrid_controller.cpp
pkg/trunk/controllers/robot_mechanism_controllers/src/endeffector_constraint_controller.cpp
pkg/trunk/demos/tabletop_manipulation/scripts/approachtable.py
pkg/trunk/demos/tabletop_manipulation/scripts/executive.py
pkg/trunk/drivers/motor/ethercat_hardware/src/pressure_node.py
pkg/trunk/drivers/robot/pr2/fingertip_pressure/scripts/pressure_visualization_marker.py
pkg/trunk/mapping/collision_map/src/collision_map_buffer.cpp
pkg/trunk/mapping/door_handle_detector/include/door_handle_detector/geometric_helper.h
pkg/trunk/mapping/door_handle_detector/src/doors_detector.cpp
pkg/trunk/mapping/door_handle_detector/src/handle_detector.cpp
pkg/trunk/mapping/door_handle_detector/src/handle_detector_vision.cpp
pkg/trunk/mapping/door_stereo_detector/src/door_stereo_detector.cpp
pkg/trunk/mapping/door_tracker/src/door_database.cpp
pkg/trunk/mapping/door_tracker/src/door_tracker.cpp
pkg/trunk/mapping/hallway_tracker/src/hallway_tracker.cpp
pkg/trunk/motion_planning/ompl_planning/src/display_planner_collision_model.cpp
pkg/trunk/motion_planning/ompl_planning/src/state_validity_monitor.cpp
pkg/trunk/nav/visual_nav/src/ros_visual_nav.cpp
pkg/trunk/vision/outlet_detection/src/outlet_spotting.cpp
pkg/trunk/vision/recognition_lambertian/src/recognition_lambertian.cpp
pkg/trunk/vision/visual_odometry/nodes/camview_py.py
pkg/trunk/vision/visual_odometry/nodes/corrector.py
pkg/trunk/vision/visual_odometry/nodes/lineperf.py
pkg/trunk/vision/visual_odometry/src/marker.py
pkg/trunk/visualization/rviz/src/rviz/displays/marker_display.cpp
pkg/trunk/visualization/rviz/src/rviz/displays/marker_display.h
pkg/trunk/visualization/rviz/src/test/marker_test.cpp
pkg/trunk/world_models/test_collision_space/src/test_cs_point_inclusion.cpp
Added Paths:
-----------
pkg/trunk/visualization_core/visualization_msgs/msg/Marker.msg
Removed Paths:
-------------
pkg/trunk/visualization_core/visualization_msgs/msg/VisualizationMarker.msg
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/head_pan_tilt_controller.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/head_pan_tilt_controller.h 2009-05-01 03:43:19 UTC (rev 14716)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/head_pan_tilt_controller.h 2009-05-01 04:47:46 UTC (rev 14717)
@@ -49,7 +49,7 @@
#include <robot_msgs/JointCmd.h>
#include <robot_srvs/GetJointCmd.h>
#include <robot_msgs/PointStamped.h>
-#include <visualization_msgs/VisualizationMarker.h>
+#include <visualization_msgs/Marker.h>
// Math utils
#include <math.h>
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/head_servoing_controller.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/head_servoing_controller.h 2009-05-01 03:43:19 UTC (rev 14716)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/head_servoing_controller.h 2009-05-01 04:47:46 UTC (rev 14717)
@@ -49,7 +49,7 @@
#include <robot_msgs/JointCmd.h>
#include <robot_srvs/GetJointCmd.h>
#include <robot_msgs/PointStamped.h>
-#include <visualization_msgs/VisualizationMarker.h>
+#include <visualization_msgs/Marker.h>
// Math utils
#include <math.h>
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/head_pan_tilt_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/head_pan_tilt_controller.cpp 2009-05-01 03:43:19 UTC (rev 14716)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/head_pan_tilt_controller.cpp 2009-05-01 04:47:46 UTC (rev 14717)
@@ -194,7 +194,7 @@
//services
node_->advertiseService(service_prefix_ + "/get_command_array", &HeadPanTiltControllerNode::getJointCmd, this);
guard_get_command_array_.set(service_prefix_ + "/get_command_array");
- node_->advertise<visualization_msgs::VisualizationMarker>( "visualizationMarker", 0 );
+ node_->advertise<visualization_msgs::Marker>( "visualization_marker", 0 );
return true;
}
@@ -340,44 +340,36 @@
c_->setJointCmd(pos,names);
- visualization_msgs::VisualizationMarker marker;
+ visualization_msgs::Marker marker;
marker.header.frame_id = "stereo";
+ marker.ns = "head_pan_tilt_controller";
marker.id = 0;
marker.type = 0;
marker.action = 0;
- marker.x = 0;
- marker.y = 0;
- marker.z = 0;
- marker.yaw = 0;
- marker.pitch = 0;
- marker.roll = 0.0;
- marker.xScale = 2;
- marker.yScale = 0.05;
- marker.zScale = 0.05;
- marker.alpha = 255;
- marker.r = 0;
- marker.g = 255;
- marker.b = 0;
- node_->publish("visualizationMarker", marker );
+ marker.pose.orientation.w = 1.0;
+ marker.scale.x = 2;
+ marker.scale.y = 0.05;
+ marker.scale.z = 0.05;
+ marker.color.g = 1.0;
+ marker.color.a = 1.0;
+ node_->publish("visualization_marker", marker );
marker.header.frame_id = "head_pan_link";
+ marker.ns = "head_pan_tilt_controller";
marker.id = 1;
marker.type = 2;
marker.action = 0;
- marker.x = pan_point.x();
- marker.y = pan_point.y();
- marker.z = pan_point.z();
- marker.yaw = 0;
- marker.pitch = 0;
- marker.roll = 0.0;
- marker.xScale = 0.1;
- marker.yScale = 0.1;
- marker.zScale = 0.1;
- marker.alpha = 255;
- marker.r = 255;
- marker.g = 0;
- marker.b = 0;
- node_->publish("visualizationMarker", marker );
+ marker.pose.position.x = pan_point.x();
+ marker.pose.position.y = pan_point.y();
+ marker.pose.position.z = pan_point.z();
+ marker.pose.orientation.w = 1.0;
+ marker.scale.x = 0.1;
+ marker.scale.y = 0.1;
+ marker.scale.z = 0.1;
+ marker.color.r = 1.0;
+ marker.color.g = 0.0;
+ marker.color.a = 1.0;
+ node_->publish("visualization_marker", marker );
}
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/head_servoing_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/head_servoing_controller.cpp 2009-05-01 03:43:19 UTC (rev 14716)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/head_servoing_controller.cpp 2009-05-01 04:47:46 UTC (rev 14717)
@@ -73,15 +73,15 @@
elt = elt->NextSiblingElement("controller");
}
-
+
TiXmlElement *cd = config->FirstChildElement("controller_defaults");
if (cd)
{
-
- //TODO: make init function for this
+
+ //TODO: make init function for this
max_velocity_ = atof(cd->Attribute("max_velocity"));
gain_ = atof(cd->Attribute("gain"));
-
+
}
else
{
@@ -93,7 +93,7 @@
fprintf(stderr,"HeadServoingController:: num_joints_: %d\n",num_joints_);
-
+
set_pts_.resize(num_joints_);
for(unsigned int i=0; i < num_joints_;++i)
{
@@ -158,8 +158,8 @@
void HeadServoingController::update(void)
{
double error(0);
-
-
+
+
for(unsigned int i=0; i < num_joints_;++i)
{
angles::shortest_angular_distance_with_limits(joint_velocity_controllers_[i]->joint_state_->position_,set_pts_[i], joint_velocity_controllers_[i]->joint_state_->joint_->joint_limit_min_, joint_velocity_controllers_[i]->joint_state_->joint_->joint_limit_max_, error);
@@ -219,7 +219,7 @@
//services
node_->advertiseService(service_prefix_ + "/get_command_array", &HeadServoingControllerNode::getJointCmd, this);
guard_get_command_array_.set(service_prefix_ + "/get_command_array");
- node_->advertise<visualization_msgs::VisualizationMarker>( "visualizationMarker", 0 );
+ node_->advertise<visualization_msgs::Marker>( "visualization_marker", 0 );
return true;
}
@@ -357,44 +357,36 @@
c_->setJointCmd(pos,names);
- visualization_msgs::VisualizationMarker marker;
+ visualization_msgs::Marker marker;
marker.header.frame_id = "stereo";
+ marker.ns = "head_servoing_controller";
marker.id = 0;
- marker.type = 0;
+ marker.type = 0;
marker.action = 0;
- marker.x = 0;
- marker.y = 0;
- marker.z = 0;
- marker.yaw = 0;
- marker.pitch = 0;
- marker.roll = 0.0;
- marker.xScale = 2;
- marker.yScale = 0.05;
- marker.zScale = 0.05;
- marker.alpha = 255;
- marker.r = 0;
- marker.g = 255;
- marker.b = 0;
- node_->publish("visualizationMarker", marker );
+ marker.pose.orientation.w = 1.0;
+ marker.scale.x = 2;
+ marker.scale.y = 0.05;
+ marker.scale.z = 0.05;
+ marker.color.g = 1.0;
+ marker.color.a = 1.0;
+ node_->publish("visualization_marker", marker );
- marker.header.frame_id = "head_pan";
+ marker.header.frame_id = "head_pan_link";
+ marker.ns = "head_servoing_controller";
marker.id = 1;
- marker.type = 2;
+ marker.type = 2;
marker.action = 0;
- marker.x = pan_point.x();
- marker.y = pan_point.y();
- marker.z = pan_point.z();
- marker.yaw = 0;
- marker.pitch = 0;
- marker.roll = 0.0;
- marker.xScale = 0.1;
- marker.yScale = 0.1;
- marker.zScale = 0.1;
- marker.alpha = 255;
- marker.r = 255;
- marker.g = 0;
- marker.b = 0;
- node_->publish("visualizationMarker", marker );
+ marker.pose.position.x = pan_point.x();
+ marker.pose.position.y = pan_point.y();
+ marker.pose.position.z = pan_point.z();
+ marker.pose.orientation.w = 1.0;
+ marker.scale.x = 0.1;
+ marker.scale.y = 0.1;
+ marker.scale.z = 0.1;
+ marker.color.r = 1.0;
+ marker.color.g = 0.0;
+ marker.color.a = 1.0;
+ node_->publish("visualization_marker", marker );
}
Modified: pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/endeffector_constraint_controller.h
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/endeffector_constraint_controller.h 2009-05-01 03:43:19 UTC (rev 14716)
+++ pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/endeffector_constraint_controller.h 2009-05-01 04:47:46 UTC (rev 14717)
@@ -49,7 +49,7 @@
#include "joy/Joy.h"
#include "Eigen/LU"
#include "Eigen/Core"
-#include <visualization_msgs/VisualizationMarker.h>
+#include <visualization_msgs/Marker.h>
namespace controller {
Modified: pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/plug_controller.h
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/plug_controller.h 2009-05-01 03:43:19 UTC (rev 14716)
+++ pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/plug_controller.h 2009-05-01 04:47:46 UTC (rev 14717)
@@ -58,7 +58,7 @@
#include "Eigen/LU"
#include "Eigen/Core"
-#include <visualization_msgs/VisualizationMarker.h>
+#include <visualization_msgs/Marker.h>
namespace controller {
Modified: pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_hybrid_controller.cpp
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_hybrid_controller.cpp 2009-05-01 03:43:19 UTC (rev 14716)
+++ pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_hybrid_controller.cpp 2009-05-01 04:47:46 UTC (rev 14717)
@@ -40,7 +40,7 @@
#include "tf_conversions/tf_kdl.h"
#include "realtime_tools/realtime_publisher.h"
#include "control_toolbox/pid.h"
-#include "visualization_msgs/VisualizationMarker.h"
+#include "visualization_msgs/Marker.h"
#include "angles/angles.h"
#include "std_msgs/Float64MultiArray.h"
Modified: pkg/trunk/controllers/robot_mechanism_controllers/src/endeffector_constraint_controller.cpp
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/src/endeffector_constraint_controller.cpp 2009-05-01 03:43:19 UTC (rev 14716)
+++ pkg/trunk/controllers/robot_mechanism_controllers/src/endeffector_constraint_controller.cpp 2009-05-01 04:47:46 UTC (rev 14717)
@@ -361,7 +361,7 @@
node_->subscribe(topic_ + "/command", wrench_msg_,
&EndeffectorConstraintControllerNode::command, this, 1);
guard_command_.set(topic_ + "/command");
- node_->advertise<visualization_msgs::VisualizationMarker>( "visualizationMarker", 0 );
+ node_->advertise<visualization_msgs::Marker>( "visualization_marker", 0 );
return true;
@@ -379,45 +379,43 @@
// Debugging code. Not currently realtime safe
- visualization_msgs::VisualizationMarker marker;
- marker.header.frame_id = "torso_lift_link";
- marker.id = 0;
- marker.type = visualization_msgs::VisualizationMarker::CUBE;
- marker.action = 0;
- marker.x = 0.6;
- marker.y = 0;
- marker.z = 0;
- marker.yaw = 0;
- marker.pitch = 0;
- marker.roll = 0.0;
- marker.xScale = 0.01;
- marker.yScale = 10;
- marker.zScale = 10;
- marker.alpha = 200;
- marker.r = 0;
- marker.g = 255;
- marker.b = 0;
- node_->publish("visualizationMarker", marker );
+ {
+ visualization_msgs::Marker marker;
+ marker.header.frame_id = "torso_lift_link";
+ marker.ns = "endeffector_constraint_controller";
+ marker.id = 0;
+ marker.type = visualization_msgs::Marker::CUBE;
+ marker.action = 0;
+ marker.pose.position.x = 0.6;
+ marker.pose.position.y = 0;
+ marker.pose.position.z = 0;
+ marker.pose.orientation.w = 1.0;
+ marker.scale.x = 0.01;
+ marker.scale.y = 10;
+ marker.scale.z = 10;
+ marker.color.g = 1.0;
+ marker.color.a = 0.7;
+ node_->publish("visualization_marker", marker );
+ }
- visualization_msgs::VisualizationMarker marker;
- marker.header.frame_id = "torso_lift_link";
- marker.id = 1;
- marker.type = visualization_msgs::VisualizationMarker::SPHERE;
- marker.action = 0;
- marker.x = 0.6;
- marker.y = 0;
- marker.z = 1;
- marker.yaw = 0;
- marker.pitch = 0;
- marker.roll = 0.0;
- marker.xScale = 0.01;
- marker.yScale = 0.4;
- marker.zScale = 0.4;
- marker.alpha = 200;
- marker.r = 255;
- marker.g = 0;
- marker.b = 0;
- node_->publish("visualizationMarker", marker );
+ {
+ visualization_msgs::Marker marker;
+ marker.header.frame_id = "torso_lift_link";
+ marker.ns = "endeffector_constraint_controller";
+ marker.id = 1;
+ marker.type = visualization_msgs::Marker::SPHERE;
+ marker.action = 0;
+ marker.pose.position.x = 0.6;
+ marker.pose.position.y = 0;
+ marker.pose.position.z = 1;
+ marker.pose.orientation.w = 1.0;
+ marker.scale.x = 0.01;
+ marker.scale.y = 0.4;
+ marker.scale.z = 0.4;
+ marker.color.r = 1.0;
+ marker.color.a = 0.7;
+ node_->publish("visualization_marker", marker );
+ }
#endif
}
Modified: pkg/trunk/demos/tabletop_manipulation/scripts/approachtable.py
===================================================================
--- pkg/trunk/demos/tabletop_manipulation/scripts/approachtable.py 2009-05-01 03:43:19 UTC (rev 14716)
+++ pkg/trunk/demos/tabletop_manipulation/scripts/approachtable.py 2009-05-01 04:47:46 UTC (rev 14717)
@@ -38,9 +38,11 @@
import rospy
from robot_srvs.srv import FindTable, FindTableRequest
from robot_msgs.msg import Polygon3D, Point
-from visualization_msgs.msg import VisualizationMarker
+from visualization_msgs.msg import Marker
from deprecated_msgs.msg import RobotBase2DOdom
from tf.msg import tfMessage
+import bullet
+import tf
from movebase import MoveBase
from detecttable import DetectTable
@@ -59,7 +61,7 @@
self.vm = None
rospy.Subscriber('odom', RobotBase2DOdom, self.odomCallback)
rospy.Subscriber('tf_message', tfMessage, self.tfCallback)
- self.pub_vis = rospy.Publisher("visualizationMarker", VisualizationMarker)
+ self.pub_vis = rospy.Publisher("visualization_marker", Marker)
self.global_frame = rospy.get_param('/global_frame_id')
# Hack to get around the fact that we don't have pytf
@@ -85,9 +87,10 @@
print '[ApproachTable] Failed to detect table'
return False
- self.vm = VisualizationMarker()
+ self.vm = Marker()
self.vm.header.frame_id = resp.table.header.frame_id
- self.vm.z = resp.table.table.points[0].z
+ self.vm.ns = "approachtable";
+ self.vm.pose.position.z = resp.table.table.points[0].z
approach_pose = self.computeApproachPose(self.robot_position, resp.table.table, standoff, True)
@@ -95,21 +98,17 @@
return False
self.vm.id = 1000
- self.vm.type = VisualizationMarker.ARROW
- self.vm.action = VisualizationMarker.ADD
- self.vm.x = approach_pose[0]
- self.vm.y = approach_pose[1]
+ self.vm.type = Marker.ARROW
+ self.vm.action = Marker.ADD
+ self.vm.pose.position.x = approach_pose[0]
+ self.vm.pose.position.y = approach_pose[1]
#self.vm.z = 0.0
- self.vm.roll = 0.0
- self.vm.pitch = 0.0
- self.vm.yaw = approach_pose[2]
- self.vm.xScale = 0.6
- self.vm.yScale = 0.25
- self.vm.zScale = 0.1
- self.vm.alpha = 128
- self.vm.r = 255
- self.vm.g = 0
- self.vm.b = 0
+ self.vm.pose.orientation = tf.quaternion_bt_to_msg(bullet.Quaternion(approach_pose[2], 0.0, 0.0))
+ self.vm.scale.x = 0.6
+ self.vm.scale.y = 0.25
+ self.vm.scale.z = 0.1
+ self.vm.color.a = 0.5
+ self.vm.color.r = 1.0
self.vm.points = []
self.pub_vis.publish(self.vm)
@@ -209,78 +208,58 @@
goala = atan2(sin(goala),cos(goala))
self.vm.id = 1001
- self.vm.type = VisualizationMarker.SPHERE
- self.vm.action = VisualizationMarker.ADD
- self.vm.x = midp.x
- self.vm.y = midp.y
+ self.vm.type = Marker.SPHERE
+ self.vm.action = Marker.ADD
+ self.vm.pose.position.x = midp.x
+ self.vm.pose.position.y = midp.y
#self.vm.z = 0.0
- self.vm.roll = 0.0
- self.vm.pitch = 0.0
- self.vm.yaw = 0.0
- self.vm.xScale = 0.05
- self.vm.yScale = 0.05
- self.vm.zScale = 0.05
- self.vm.alpha = 128
- self.vm.r = 0
- self.vm.g = 0
- self.vm.b = 255
+ self.vm.pose.orientation.x = 0.0
+ self.vm.pose.orientation.y = 0.0
+ self.vm.pose.orientation.z = 0.0
+ self.vm.pose.orientation.w = 1.0
+ self.vm.scale.x = 0.05
+ self.vm.scale.y = 0.05
+ self.vm.scale.z = 0.05
+ self.vm.color.a = 0.5
+ self.vm.color.r = 0.0
+ self.vm.color.g = 0.0
+ self.vm.color.b = 1.0
self.vm.points = []
self.pub_vis.publish(self.vm)
self.vm.id = 1002
- self.vm.type = VisualizationMarker.SPHERE
- self.vm.action = VisualizationMarker.ADD
- self.vm.x = closestp[0].x
- self.vm.y = closestp[0].y
+ self.vm.type = Marker.SPHERE
+ self.vm.action = Marker.ADD
+ self.vm.pose.position.x = closestp[0].x
+ self.vm.pose.position.y = closestp[0].y
#self.vm.z = 0.0
- self.vm.roll = 0.0
- self.vm.pitch = 0.0
- self.vm.yaw = 0.0
- self.vm.xScale = 0.05
- self.vm.yScale = 0.05
- self.vm.zScale = 0.05
- self.vm.alpha = 128
- self.vm.r = 0
- self.vm.g = 255
- self.vm.b = 0
+ self.vm.color.r = 0.0
+ self.vm.color.g = 1.0
+ self.vm.color.b = 0.0
self.vm.points = []
self.pub_vis.publish(self.vm)
self.vm.id = 1003
- self.vm.type = VisualizationMarker.SPHERE
- self.vm.action = VisualizationMarker.ADD
- self.vm.x = closestp[1].x
- self.vm.y = closestp[1].y
+ self.vm.type = Marker.SPHERE
+ self.vm.action = Marker.ADD
+ self.vm.pose.position.x = closestp[1].x
+ self.vm.pose.position.y = closestp[1].y
#self.vm.z = 0.0
- self.vm.roll = 0.0
- self.vm.pitch = 0.0
- self.vm.yaw = 0.0
- self.vm.xScale = 0.05
- self.vm.yScale = 0.05
- self.vm.zScale = 0.05
- self.vm.alpha = 128
- self.vm.r = 255
- self.vm.g = 0
- self.vm.b = 255
+ self.vm.color.r = 1.0
+ self.vm.color.g = 0.0
+ self.vm.color.b = 1.0
self.vm.points = []
self.pub_vis.publish(self.vm)
self.vm.id = 1004
- self.vm.type = VisualizationMarker.SPHERE
- self.vm.action = VisualizationMarker.ADD
- self.vm.x = farthestp.x
- self.vm.y = farthestp.y
+ self.vm.type = Marker.SPHERE
+ self.vm.action = Marker.ADD
+ self.vm.pose.position.x = farthestp.x
+ self.vm.pose.position.y = farthestp.y
#self.vm.z = 0.0
- self.vm.roll = 0.0
- self.vm.pitch = 0.0
- self.vm.yaw = 0.0
- self.vm.xScale = 0.05
- self.vm.yScale = 0.05
- self.vm.zScale = 0.05
- self.vm.alpha = 128
- self.vm.r = 255
- self.vm.g = 255
- self.vm.b = 255
+ self.vm.color.r = 1.0
+ self.vm.color.g = 01
+ self.vm.color.b = 1.0
self.vm.points = []
self.pub_vis.publish(self.vm)
Modified: pkg/trunk/demos/tabletop_manipulation/scripts/executive.py
===================================================================
--- pkg/trunk/demos/tabletop_manipulation/scripts/executive.py 2009-05-01 03:43:19 UTC (rev 14716)
+++ pkg/trunk/demos/tabletop_manipulation/scripts/executive.py 2009-05-01 04:47:46 UTC (rev 14717)
@@ -125,7 +125,7 @@
import rospy
import random
import sys
-from visualization_msgs.msg import VisualizationMarker
+from visualization_msgs.msg import Marker
from robot_msgs.msg import AttachedObject, PoseConstraint
from robot_srvs.srv import FindTable, FindTableRequest, SubtractObjectFromCollisionMap, SubtractObjectFromCollisionMapRequest, RecordStaticMapTrigger, RecordStaticMapTriggerRequest
from pr2_mechanism_controllers.srv import SetProfile, SetProfileRequest
@@ -165,7 +165,7 @@
self.laser_tilt_profile_amplitude = 0.75
self.laser_tilt_profile_offset = 0.30
- self.vis_pub = rospy.Publisher("visualizationMarker", VisualizationMarker)
+ self.vis_pub = rospy.Publisher("visualization_marker", Marker)
self.attached_obj_pub = rospy.Publisher("attach_object", AttachedObject)
print 'Waiting for service: ' + self.laser_tilt_profile_controller + '/set_profile'
@@ -239,22 +239,22 @@
def drawObjectVisMarker(self, obj):
# TODO: use bounds instead of this hardcoded radius
radius = 2.0
-
- mk = VisualizationMarker()
+ mk = Marker()
mk.header.frame_id = obj.frame_id
+ mk.ns = "executive"
mk.id = self.vmk_id
#self.vmk_id += 1
- mk.type = VisualizationMarker.SPHERE
- mk.action = VisualizationMarker.ADD # same as modify
- mk.x = obj.center.x
- mk.y = obj.center.y
- mk.z = obj.center.z
- mk.roll = mk.pitch = mk.yaw = 0
- mk.xScale = mk.yScale = mk.zScale = radius * 2.0
- mk.alpha = 255
- mk.r = 255
- mk.g = 10
- mk.b = 10
+ mk.type = Marker.SPHERE
+ mk.action = Marker.ADD # same as modify
+ mk.pose.position.x = obj.center.x
+ mk.pose.position.y = obj.center.y
+ mk.pose.position.z = obj.center.z
+ mk.pose.orientation.w = 1.0
+ mk.scale.x = mk.scale.y = mk.scale.z = radius * 2.0
+ mk.color.a = 1.0
+ mk.color.r = 1.0
+ mk.color.g = 0.04
+ mk.color.b = 0.04
vis_pub.publish(mk)
Modified: pkg/trunk/drivers/motor/ethercat_hardware/src/pressure_node.py
===================================================================
--- pkg/trunk/drivers/motor/ethercat_hardware/src/pressure_node.py 2009-05-01 03:43:19 UTC (rev 14716)
+++ pkg/trunk/drivers/motor/ethercat_hardware/src/pressure_node.py 2009-05-01 04:47:46 UTC (rev 14717)
@@ -37,14 +37,14 @@
# Author: Blaise Gassend
# Reads fingertip pressure data from /pressure and publishes it as a
-# visualizationMarker
+# visualization_marker
import roslib
roslib.load_manifest('ethercat_hardware')
import rospy
from ethercat_hardware.msg import PressureState
-from visualization_msgs.msg import VisualizationMarker
+from visualization_msgs.msg import Marker
positions = [ # x, y, z, xscale, yscale, zscale
( 0.026, 0.007, 0.000, 0.010, 0.010, 0.015),
@@ -77,14 +77,14 @@
return (0,0,0)
if data < 3000:
x = (data-1000)/2000.
- return 0,0,255*x
+ return 0,0,x
if data < 6000:
x = (data-3000)/3000.
- return 255*x,0,255*(1-x)
+ return x,0,(1-x)
if data < 10000:
x = (data-6000)/4000.
- return 255,255*x,255*x
- return 255,255,255
+ return 1.0,x,x
+ return 1.0,1.0,1.0
class pressureVisualizer:
def callback(self, pressurestate):
@@ -101,21 +101,22 @@
self.makeVisualization(self.data1, self.frame1, self.startid + numsensors,-1)
def makeVisualization(self, data, frame, sensorstartid, ydir):
- mk = VisualizationMarker()
+ mk = Marker()
mk.header.frame_id = frame
# @todo Make timestamp come from when data is collected in hardware
- mk.header.stamp = rospy.get_rostime()
- mk.type = VisualizationMarker.SPHERE
- mk.action = VisualizationMarker.ADD
+ mk.header.stamp = rospy.get_rostime()
+ mk.ns = "pressure_node"
+ mk.type = Marker.SPHERE
+ mk.action = Marker.ADD
mk.points = []
for i in range(0,numsensors):
mk.id = sensorstartid + i
- (mk.x, mk.y, mk.z, mk.xScale, mk.yScale, mk.zScale) = positions[i]
- mk.y = mk.y * ydir
- mk.z = mk.z * ydir
- mk.roll = mk.pitch = mk.yaw = 0
- mk.alpha = 255
- (mk.r, mk.g, mk.b) = color(data[i])
+ (mk.pose.positions.x, mk.pose.positions.y, mk.pose.positions.z, mk.scale.x, mk.scale.y, mk.scale.z) = positions[i]
+ mk.pose.positions.y = mk.pose.positions.y * ydir
+ mk.pose.positions.z = mk.pose.positions.z * ydir
+ mk.pose.orientation.w = 1.0
+ mk.color.a = 1.0
+ (mk.color.r, mk.color.g, mk.color.b) = color(data[i])
self.vis_pub.publish(mk)
def __init__(self, source, frame0, frame1, startid):
@@ -126,8 +127,8 @@
rospy.init_node('pressureVisualizer', anonymous=True)
- self.vis_pub = rospy.Publisher('visualizationMarker',
- VisualizationMarker)
+ self.vis_pub = rospy.Publisher('visualization_marker',
+ Marker)
rospy.Subscriber(source, PressureState, self.callback)
while not rospy.is_shutdown():
Modified: pkg/trunk/drivers/robot/pr2/fingertip_pressure/scripts/pressure_visualization_marker.py
===================================================================
--- pkg/trunk/drivers/robot/pr2/fingertip_pressure/scripts/pressure_visualization_marker.py 2009-05-01 03:43:19 UTC (rev 14716)
+++ pkg/trunk/drivers/robot/pr2/fingertip_pressure/scripts/pressure_visualization_marker.py 2009-05-01 04:47:46 UTC (rev 14717)
@@ -37,14 +37,14 @@
# Author: Blaise Gassend
# Reads fingertip pressure data from /pressure and publishes it as a
-# visualizationMarker
+# visualization_marker
import roslib
roslib.load_manifest('fingertip_pressure')
import rospy
from ethercat_hardware.msg import PressureState
-from visualization_msgs.msg import VisualizationMarker
+from visualization_msgs.msg import Marker
positions = [ # x, y, z, xscale, yscale, zscale
( 0.026, 0.007, 0.000, 0.010, 0.010, 0.015),
@@ -77,14 +77,14 @@
return (0,0,0)
if data < 3000:
x = (data-1000)/2000.
- return 0,0,255*x
+ return 0,0,x
if data < 6000:
x = (data-3000)/3000.
- return 255*x,0,255*(1-x)
+ return x,0,(1-x)
if data < 10000:
x = (data-6000)/4000.
- return 255,255*x,255*x
- return 255,255,255
+ return 1.0,x,x
+ return 1.0,1.0,1.0
class pressureVisualizer:
def callback(self, pressurestate):
@@ -101,21 +101,22 @@
self.makeVisualization(self.data1, self.frame1, self.startid + numsensors,-1)
def makeVisualization(self, data, frame, sensorstartid, ydir):
- mk = VisualizationMarker()
+ mk = Marker()
mk.header.frame_id = frame
# @todo Make timestamp come from when data is collected in hardware
- mk.header.stamp = rospy.get_rostime()
- mk.type = VisualizationMarker.SPHERE
- mk.action = VisualizationMarker.ADD
+ mk.header.stamp = rospy.get_rostime()
+ mk.ns = "pressure_node"
+ mk.type = Marker.SPHERE
+ mk.action = Marker.ADD
mk.points = []
for i in range(0,numsensors):
mk.id = sensorstartid + i
- (mk.x, mk.y, mk.z, mk.xScale, mk.yScale, mk.zScale) = positions[i]
- mk.y = mk.y * ydir
- mk.z = mk.z * ydir
- mk.roll = mk.pitch = mk.yaw = 0
- mk.alpha = 255
- (mk.r, mk.g, mk.b) = color(data[i])
+ (mk.pose.positions.x, mk.pose.positions.y, mk.pose.positions.z, mk.scale.x, mk.scale.y, mk.scale.z) = positions[i]
+ mk.pose.positions.y = mk.pose.positions.y * ydir
+ mk.pose.positions.z = mk.pose.positions.z * ydir
+ mk.pose.orientation.w = 1.0
+ mk.color.a = 1.0
+ (mk.color.r, mk.color.g, mk.color.b) = color(data[i])
self.vis_pub.publish(mk)
def __init__(self, source, frame0, frame1, startid):
@@ -126,8 +127,8 @@
rospy.init_node('pressureVisualizer', anonymous=True)
- self.vis_pub = rospy.Publisher('visualizationMarker',
- VisualizationMarker)
+ self.vis_pub = rospy.Publisher('visualization_marker',
+ Marker)
rospy.Subscriber(source, PressureState, self.callback)
while not rospy.is_shutdown():
Modified: pkg/trunk/mapping/collision_map/src/collision_map_buffer.cpp
===================================================================
--- pkg/trunk/mapping/collision_map/src/collision_map_buffer.cpp 2009-05-01 03:43:19 UTC (rev 14716)
+++ pkg/trunk/mapping/collision_map/src/collision_map_buffer.cpp 2009-05-01 04:47:46 UTC (rev 14717)
@@ -46,7 +46,7 @@
#include <robot_msgs/Point.h>
#include <robot_msgs/PointCloud.h>
#include <robot_msgs/PoseStamped.h>
-#include <visualization_msgs/VisualizationMarker.h>
+#include <visualization_msgs/Marker.h>
#include <Eigen/Core>
#include <point_cloud_mapping/geometry/point.h>
@@ -188,7 +188,7 @@
subtract_object_ = false;
m_id_ = 0;
- node_.advertise<visualization_msgs::VisualizationMarker>("visualizationMarker", 100);
+ node_.advertise<visualization_msgs::Marker>("visualization_marker", 100);
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
@@ -375,27 +375,28 @@
void
sendMarker (Point32 pt, const std::string &frame_id, double radius = 0.02)
{
- VisualizationMarker mk;
+ Marker mk;
mk.header.stamp = ros::Time::now();
mk.header.frame_id = frame_id;
+ mk.ns = "collision_map_buffer";
mk.id = ++m_id_;
- mk.type = VisualizationMarker::SPHERE;
- mk.action = VisualizationMarker::ADD;
- mk.x = pt.x;
- mk.y = pt.y;
- mk.z = pt.z;
+ mk.type = Marker::SPHERE;
+ mk.action = Marker::ADD;
+ mk.pose.position.x = pt.x;
+ mk.pose.position.y = pt.y;
+ mk.pose.position.z = pt.z;
- mk.roll = mk.pitch = mk.yaw = 0;
- mk.xScale = mk.yScale = mk.zScale = radius * 2.0;
+ mk.pose.orientation.w = 1.0;
+ mk.scale.x = mk.scale.y = mk.scale.z = radius * 2.0;
- mk.alpha = 255;
- mk.r = 255;
- mk.g = 10;
- mk.b = 10;
+ mk.color.a = 1.0;
+ mk.color.r = 1.0;
+ mk.color.g = 0.04;
+ mk.color.b = 0.04;
- node_.publish ("visualizationMarker", mk);
+ node_.publish ("visualization_marker", mk);
}
Modified: pkg/trunk/mapping/door_handle_detector/include/door_handle_detector/geometric_helper.h
===================================================================
--- pkg/trunk/mapping/door_handle_detector/include/door_handle_detector/geometric_helper.h 2009-05-01 03:43:19 UTC (rev 14716)
+++ pkg/trunk/mapping/door_handle_detector/include/door_handle_detector/geometric_helper.h 2009-05-01 04:47:46 UTC (rev 14717)
@@ -41,7 +41,7 @@
#include <robot_msgs/Polygon3D.h>
#include <robot_msgs/Point32.h>
#include <robot_msgs/PointStamped.h>
-#include <visualization_msgs/VisualizationMarker.h>
+#include <visualization_msgs/Marker.h>
#include <robot_msgs/Door.h>
#include <tf/transform_listener.h>
@@ -155,27 +155,26 @@
sendMarker (float px, float py, float pz, std::string frame_id, ros::Node *anode, int &id,
int r, int g, int b, double radius = 0.03)
{
- visualization_msgs::VisualizationMarker mk;
+ visualization_msgs::Marker mk;
mk.header.stamp = ros::Time::now ();
mk.header.frame_id = frame_id;
mk.id = id++;
- mk.type = visualization_msgs::VisualizationMarker::SPHERE;
- mk.action = visualization_msgs::VisualizationMarker::ADD;
- mk.x = px;
- mk.y = py;
- mk.z = pz;
+ mk.ns = "geometric_helper";
+ mk.type = visualization_msgs::Marker::SPHERE;
+ mk.action = visualization_msgs::Marker::ADD;
+ mk.pose.position.x = px;
+ mk.pose.position.y = py;
+ mk.pose.position.z = pz;
+ mk.pose.orientation.w = 1.0;
+ mk.scale.x = mk.scale.y = mk.scale.z = radius * 2.0;
+ mk.color.a = 1.0;
+ mk.color.r = r;
+ mk.color.g = g;
+ mk.color.b = b;
- mk.roll = mk.pitch = mk.yaw = 0;
- mk.xScale = mk.yScale = mk.zScale = radius * 2.0;
-
- mk.alpha = 255;
- mk.r = r;
- mk.g = g;
- mk.b = b;
-
- anode->publish ("visualizationMarker", mk);
+ anode->publish ("visualization_marker", mk);
}
void obtainCloudIndicesSet (robot_msgs::PointCloud *points, std::vector<int> &indices, robot_msgs::Door& door,
Modified: pkg/trunk/mapping/door_handle_detector/src/doors_detector.cpp
===================================================================
--- pkg/trunk/mapping/door_handle_detector/src/doors_detector.cpp 2009-05-01 03:43:19 UTC (rev 14716)
+++ pkg/trunk/mapping/door_handle_detector/src/doors_detector.cpp 2009-05-01 04:47:46 UTC (rev 14717)
@@ -104,7 +104,7 @@
node_->param ("~input_cloud_topic", input_cloud_topic_, string ("/snapshot_cloud"));
node_->advertiseService ("doors_detector", &DoorDetector::detectDoorSrv, this);
node_->advertiseService ("doors_detector_cloud", &DoorDetector::detectDoorCloudSrv, this);
- node_->advertise<visualization_msgs::VisualizationMarker> ("visualizationMarker", 100);
+ node_->advertise<visualization_msgs::Marker> ("visualization_marker", 100);
node_->advertise<PolygonalMap> ("~door_frames", 1);
node_->advertise<PointCloud> ("~door_regions", 1);
@@ -469,7 +469,7 @@
// transform door message
if (!transformTo(tf_, fixed_frame_, result[nr_d], result[nr_d], fixed_frame_)){
- ROS_ERROR ("DoorsDetector: could not tranform door from '%s' to '%s' at time %f",
+ ROS_ERROR ("DoorsDetector: could not tranform door from '%s' to '%s' at time %f",
result[nr_d].header.frame_id.c_str(), fixed_frame_.c_str(), result[nr_d].header.stamp.toSec());
return false;
}
Modified: pkg/trunk/mapping/door_handle_detector/src/handle_detector.cpp
===================================================================
--- pkg/trunk/mapping/door_handle_detector/src/handle_detector.cpp 2009-05-01 03:43:19 UTC (rev 14716)
+++ pkg/trunk/mapping/door_handle_detector/src/handle_detector.cpp 2009-05-01 04:47:46 UTC (rev 14717)
@@ -67,7 +67,7 @@
node_->param ("~input_cloud_topic", input_cloud_topic_, string ("/snapshot_cloud"));
node_->advertiseService ("handle_detector", &HandleDetector::detectHandleSrv, this);
node_->advertiseService ("handle_detector_cloud", &HandleDetector::detectHandleCloudSrv, this);
- node_->advertise<visualization_msgs::VisualizationMarker> ("visualizationMarker", 100);
+ node_->advertise<visualization_msgs::Marker> ("visualization_marker", 100);
node_->advertise<PolygonalMap> ("~handle_polygon", 1);
node_->advertise<PointCloud> ("~handle_regions", 1);
@@ -270,7 +270,7 @@
pmap.polygons[0] = polygon;
node_->publish ("~handle_polygon", pmap);
- // Reply door message
+ // Reply door message
result.resize(1);
result[0] = door_tr;
result[0].handle = handle_center;
Modified: pkg/trunk/mapping/door_handle_detector/src/handle_detector_vision.cpp
===================================================================
--- pkg/trunk/mapping/door_handle_detector/src/handle_detector_vision.cpp 2009-05-01 03:43:19 UTC (rev 14716)
+++ pkg/trunk/mapping/door_handle_detector/src/handle_detector_vision.cpp 2009-05-01 04:47:46 UTC (rev 14717)
@@ -57,7 +57,7 @@
#include "robot_msgs/Point32.h"
#include "robot_msgs/PointStamped.h"
#include "robot_msgs/Door.h"
-#include "visualization_msgs/VisualizationMarker.h"
+#include "visualization_msgs/Marker.h"
#include "door_handle_detector/DoorsDetector.h"
#include "std_srvs/Empty.h"
@@ -189,7 +189,7 @@
// invalid location until we get a detection
// advertise<robot_msgs::PointStamped>("handle_detector/handle_location", 1);
- advertise<visualization_msgs::VisualizationMarker>("visualizationMarker", 1);
+ advertise<visualization_msgs::Marker>("visualization_marker", 1);
advertiseService("door_handle_vision_detector", &HandleDetector::detectHandleSrv, this);
advertiseService("door_handle_vision_preempt", &HandleDetector::preempt, this);
}
@@ -208,7 +208,7 @@
if(storage){
cvReleaseMemStorage(&storage);
}
- unadvertise("visualizationMarker");
+ unadvertise("visualization_marker");
unadvertiseService("door_handle_vision_detector");
unadvertiseService("door_handle_vision_preempt");
}
@@ -384,26 +384,21 @@
*/
void showHandleMarker(robot_msgs::PointStamped p)
{
- visualization_msgs::VisualizationMarker marker;
+ visualization_msgs::Marker marker;
marker.header.frame_id = p.header.frame_id;
- marker.header.stamp = ros::Time((uint64_t)(0ULL));
+ marker.header.stamp = ros::Time();
+ marker.ns = "handle_detector_vision";
marker.id = 0;
- marker.type = visualization_msgs::VisualizationMarker::SPHERE;
- marker.action = visualization_msgs::VisualizationMarker::ADD;
- marker.x = p.point.x;
- marker.y = p.point.y;
- marker.z = p.point.z;
- marker.yaw = 0.0;
- marker.pitch = 0.0;
- marker.roll = 0.0;
- marker.xScale = 0.1;
- marker.yScale = 0.1;
- marker.zScale = 0.1;
- marker.alpha = 255;
- marker.r = 0;
- marker.g = 255;
- marker.b = 0;
- publish("visualizationMarker", marker);
+ marker.type = visualization_msgs::Marker::SPHERE;
+ marker.action = visualization_msgs::Marker::ADD;
+ marker.pose.position.x = p.point.x;
+ marker.pose.position.y = p.point.y;
+ marker.pose.position.z = p.point.z;
+ marker.pose.orientation.w = 1.0;
+ marker.scale.x = marker.scale.y = marker.scale.z = 0.1;
+ marker.color.g = 1.0;
+ marker.color.a = 1.0;
+ publish("visualization_marker", marker);
}
@@ -441,7 +436,7 @@
// marker.r = 255;
// marker.g = 0;
// marker.b = 0;
-// publish("visualizationMarker", marker);
+// publish("visualization_marker", marker);
//
// printf("Show marker at: (%f,%f,%f)", marker.x, marker.y, marker.z);
// }
Modified: pkg/trunk/mapping/door_stereo_detector/src/door_stereo_detector.cpp
===================================================================
--- pkg/trunk/mapping/door_stereo_detector/src/door_stereo_detector.cpp 2009-05-01 03:43:19 UTC (rev 14716)
+++ pkg/trunk/mapping/door_stereo_detector/src/door_stereo_detector.cpp 2009-05-01 04:47:46 UTC (rev 14717)
@@ -50,7 +50,7 @@
#include <robot_msgs/Point32.h>
#include <robot_msgs/Door.h>
-#include <visualization_msgs/VisualizationMarker.h>
+#include <visualization_msgs/Marker.h>
#include "opencv_latest/CvBridge.h"
@@ -177,7 +177,7 @@
cvNamedWindow("disparity", CV_WINDOW_AUTOSIZE);
}
- advertise<visualization_msgs::VisualizationMarker>( "visualizationMarker", 0 );
+ advertise<visualization_msgs::Marker>( "visualization_marker", 0 );
advertise<robot_msgs::PointCloud>( "filtered_cloud", 0 );
subscribeStereoData();
@@ -186,7 +186,7 @@
~DoorStereo()
{
- unadvertise("visualizationMarker");
+ unadvertise("visualization_marker");
unsubscribeStereoData();
}
@@ -259,25 +259,19 @@
//Publish all the lines as visualization markers
for(unsigned int i=0; i < inliers.size(); i++)
{
- visualization_msgs::VisualizationMarker marker;
+ visualization_msgs::Marker marker;
marker.header.frame_id = cloud.header.frame_id;
marker.header.stamp = ros::Time((uint64_t)0ULL);
+ marker.ns = "door_stereo_detector";
marker.id = i;
- marker.type = visualization_msgs::VisualizationMarker::LINE_STRIP;
- marker.action = visualization_msgs::VisualizationMarker::ADD;
- marker.x = 0.0;
- marker.y = 0.0;
- marker.z = 0.0;
- marker.yaw = 0.0;
- marker.pitch = 0.0;
- marker.roll = 0.0;
- marker.xScale = 0.01;
- marker.yScale = 0.1;
- marker.zScale = 0.1;
- marker.alpha = 255;
- marker.r = 0;
- marker.g = 255;
- marker.b = 0;
+ marker.type = visualization_msgs::Marker::LINE_STRIP;
+ marker.action = visualization_msgs::Marker::ADD;
+ marker.pose.orientation.w = 1.0;
+ marker.scale.x = 0.01;
+ marker.scale.y = 0.1;
+ marker.scale.z = 0.1;
+ marker.color.a = 1.0;
+ marker.color.g = 1.0;
marker.set_points_size(2);
marker.points[0].x = line_segment_min[i].x;
@@ -288,7 +282,7 @@
marker.points[1].y = line_segment_max[i].y;
marker.points[1].z = line_segment_max[i].z;
- publish( "visualizationMarker", marker );
+ publish( "visualization_marker", marker );
}
}
Modified: pkg/trunk/mapping/door_tracker/src/door_database.cpp
===================================================================
--- pkg/trunk/mapping/door_tracker/src/door_database.cpp 2009-05-01 03:43:19 UTC (rev 14716)
+++ pkg/trunk/mapping/door_tracker/src/door_database.cpp 2009-05-01 04:47:46 UTC (rev 14717)
@@ -47,7 +47,7 @@
#include <robot_msgs/Point32.h>
#include <robot_msgs/Door.h>
-#include <visualization_msgs/VisualizationMarker.h>
+#include <visualization_msgs/Marker.h>
#include <tf/transform_listener.h>
#include <tf/message_notifier.h>
@@ -100,12 +100,12 @@
tf_ = new tf::TransformListener(*node_);
node_->param<std::string>("~p_door_msg_topic_", door_msg_topic_,"/door_tracker_node/door_message");
- node_->param<std::string>("~door_database_frame", door_database_frame_,"map");
- node_->param<int>("~p_min_angles_per_door",min_angles_per_door_, 4);
+ node_->param<std::string>("~door_database_frame", door_database_frame_,"map");
+ node_->param<int>("~p_min_angles_per_door",min_angles_per_door_, 4);
node_->param<double>("~p_angle_difference_threshold",angle_difference_threshold_,M_PI/12.0);
node_->param<double>("~p_door_point_distance_threshold",door_point_distance_threshold_,0.25);
- node_->advertise<visualization_msgs::VisualizationMarker>( "visualizationMarker", 0 );
+ node_->advertise<visualization_msgs::Marker>( "visualization_marker", 0 );
double tmp; int tmp2;
@@ -124,7 +124,7 @@
~DoorDatabase()
{
- node_->unadvertise("visualizationMarker");
+ node_->unadvertise("visualization_marker");
delete message_notifier_;
}
@@ -157,7 +157,7 @@
lhs(i,2) = -x1*db.coeff[i].data[0]*cos(db.angles[i]) + y1*db.coeff[i].data[0]*sin(db.angles[i])-y1*db.coeff[i].data[1]*cos(db.angles[i])-x1*db.coeff[i].data[1]*sin(db.angles[i]);
ROS_INFO(" Matrix elements are: %f %f %f %f",lhs(i,0),lhs(i,1),lhs(i,2),db.angles[i]);
}
-
+
VectorXf rhs;
rhs = VectorXf::Zero((int) db.angles.size());
for(int i=0; i< (int) db.angles.size(); i++)
@@ -194,7 +194,7 @@
void updateDatabase(const robot_msgs::Door &door)
{
// (a) check if it's already in the database
- // (b) Add it in if it's not
+ // (b) Add it in if it's not
if(!inDatabase(database_,door))
addToDatabase(database_,door);
}
@@ -215,7 +215,7 @@
database[index].angles[0] = door_angle;
database[index].set_coeff_size(1);
database[index].coeff[0];
- database[index].coeff[0] = generateLinearCoeff(door.door_p1,door.door_p2);
+ database[index].coeff[0] = generateLinearCoeff(door.door_p1,door.door_p2);
ROS_INFO("Adding new door to database: p1: %f, %f, %f, p2: %f, %f, %f",door.door_p1.x,door.door_p1.y,door.door_p1.z,door.door_p2.x,door.door_p2.y,door.door_p2.z);
ROS_INFO("There are %d candidate doors in the database now.",index+1);
}
@@ -299,29 +299,26 @@
}
void publishPoint(const Point32 &point, const int &id, const std::string &frame_id)
- {
- visualization_msgs::VisualizationMarker marker;
+ {
+ visualization_msgs::Marker marker;
marker.header.frame_id = frame_id;
marker.header.stamp = ros::Time();
+ marker.ns = "door_database";
marker.id = id;
- marker.type = visualization_msgs::VisualizationMarker::SPHERE;
- marker.action = visualization_msgs::VisualizationMarker::ADD;
- marker.x = point.x;
- marker.y = point.y;
- marker.z = point.z;
- marker.yaw = 0.0;
- marker.pitch = 0.0;
- marker.roll = 0.0;
- marker.xScale = 0.1;
- marker.yScale = 0.1;
- marker.zScale = 0.1;
- marker.alpha = 255;
- marker.r = 0;
- marker.g = 255;
- marker.b = 0;
+ marker.type = visualization_msgs::Marker::SPHERE;
+ marker.action = visualization_msgs::Marker::ADD;
+ marker.pose.position.x = point.x;
+ marker.pose.position.y = point.y;
+ marker.pose.position.z = point.z;
+ marker.pose.orientation.w = 1.0;
+ marker.scale.x = 0.1;
+ marker.scale.y = 0.1;
+ marker.scale.z = 0.1;
+ marker.color.a = 1.0;
+ marker.color.g = 1.0;
// ROS_DEBUG("Publishing line between: p1: %f %f %f, p2: %f %f %f",marker.points[0].x,marker.points[0].y,marker.points[0].z,marker.points[1].x,marker.points[1].y,marker.points[1].z);
- node_->publish( "visualizationMarker", marker );
+ node_->publish( "visualization_marker", marker );
}
void publishDoors()
@@ -354,7 +351,7 @@
{
hinge_index_p1 = 1;
hinge_index_p2 = 0;
- return true;
+ return true;
}
else if(distance(door1.door_p2,door2.door_p2) < door_point_distance_threshold_)
{
Modified: pkg/trunk/mapping/door_tracker/src/door_tracker.cpp
===================================================================
--- pkg/trunk/mapping/door_tracker/src/door_tracker.cpp 2009-05-01 03:43:19 UTC (rev 14716)
+++ pkg/trunk/mapping/door_tracker/src/door_tracker.cpp 2009-05-01 04:47:46 UTC (rev 14717)
@@ -52,7 +52,7 @@
#include <robot_msgs/Point32.h>
#include <robot_msgs/Door.h>
-#include <visualization_msgs/VisualizationMarker.h>
+#include <visualization_msgs/Marker.h>
#include <std_msgs/String.h>
@@ -345,10 +345,10 @@
node_->param("~p_door_rot_dir" , tmp2, -1); door_msg_.rot_dir = tmp2;
door_msg_.header.frame_id = "base_link";
- node_->param("~publish_all_candidates" , publish_all_candidates_, false);
+ node_->param("~publish_all_candidates" , publish_all_candidates_, false);
// node_->subscribe(door_msg_topic_,door_msg_in_, &DoorTracker::doorMsgCallBack,this,1);
- node_->advertise<visualization_msgs::VisualizationMarker>( "visualizationMarker", 0 );
+ node_->advertise<visualization_msgs::Marker>( "visualization_marker", 0 );
node_->advertise<robot_msgs::Door>( "~door_message", 0 );
node_->advertiseService ("~doors_detector", &DoorTracker::detectDoorService, this);
@@ -367,7 +367,7 @@
// node_->unsubscribe(door_msg_topic_,&DoorTracker::doorMsgCallBack,this);
node_->unadvertise("~doors_detector");
node_->unadvertise("~door_message");
- node_->unadvertise("visualizationMarker");
+ node_->unadvertise("visualization_marker");
delete message_notifier_;
delete tf_;
}
@@ -456,26 +456,20 @@
}
void publishLine(const Point32 &min_p, const Point32 &max_p, const int &id, const std::string &frame_id)
- {
- visualization_msgs::VisualizationMarker marker;
+ {
+ visualization_msgs::Marker marker;
marker.header.frame_id = frame_id;
marker.header.stamp = ros::Time();
+ marker.ns = "door_tracker";
marker.id = id;
- marker.type = visualization_msgs::VisualizationMarker::LINE_STRIP;
- marker.action = visualization_msgs::VisualizationMarker::ADD;
- marker.x = 0.0;
- marker.y = 0.0;
- marker.z = 0.0;
- marker.yaw = 0.0;
- marker.pitch = 0.0;
- marker.roll = 0.0;
- marker.xScale = 0.01;
- marker.yScale = 0.1;
- marker.zScale = 0.1;
- marker.alpha = 255;
- marker.r = 0;
- marker.g = 0;
- marker.b = 255;
+ marker.type = visualization_msgs::Marker::LINE_STRIP;
+ marker.action = visualization_msgs::Marker::ADD;
+ marker.pose.orientation.w = 1.0;
+ marker.scale.x = 0.01;
+ marker.scale.y = 0.1;
+ marker.scale.z = 0.1;
+ marker.color.a = 1.0;
+ marker.color.b = 1.0;
marker.set_points_size(2);
marker.points[0].x = min_p.x;
@@ -487,7 +481,7 @@
marker.points[1].z = max_p.z;
ROS_DEBUG("Publishing line between: p1: %f %f %f, p2: %f %f %f",marker.points[0].x,marker.points[0].y,marker.points[0].z,marker.points[1].x,marker.points[1].y,marker.points[1].z);
- node_->publish( "visualizationMarker", marker );
+ node_->publish( "visualization_marker", marker );
}
@@ -546,11 +540,11 @@
ROS_DEBUG("This candidate line has the wrong width: %f which is outside the (min,max) limits: (%f,%f)",door_frame_width,door_min_width_,door_max_width_);
continue;
}
-
+
Point32 temp_min,temp_max;
transform2DInverse(line_segment_min[i],temp_min,global_x_,global_y_,global_yaw_);
transform2DInverse(line_segment_max[i],temp_max,global_x_,global_y_,global_yaw_);
-
+
double door_pt_angle_1 = atan2(temp_max.y,temp_max.x);
double door_pt_angle_2 = atan2(temp_min.y,temp_min.x);
@@ -582,7 +576,7 @@
door_tmp.door_p2.z = line_segment_max[inliers_size_max_index].z;
door_tmp.header = cloud.header;
door_tmp.header.frame_id = fixed_frame_;
- node_->publish( "~door_message", door_tmp);
+ node_->publish( "~door_message", door_tmp);
}
ROS_DEBUG("Done finding door");
@@ -677,7 +671,7 @@
vector<int> inliers_local;
model->selectWithinDistance(new_coeff, sac_distance_threshold_,inliers_local);
-
+
// Split the inliers into clusters
vector<vector<int> > clusters;
findClusters (*points, inliers_local, euclidean_cluster_distance_tolerance_, clusters, -1, 1, 2,
Modified: pkg/trunk/mapping/hallway_tracker/src/hallway_tracker.cpp
===================================================================
--- pkg/trunk/mapping/hallway_tracker/src/hallway_tracker.cpp 2009-05-01 03:43:19 UTC (rev 14716)
+++ pkg/trunk/mapping/hallway_tracker/src/hallway_tracker.cpp 2009-05-01 04:47:46 UTC (rev 14717)
@@ -53,7 +53,7 @@
#include <robot_msgs/Point32.h>
//#include <robot_msgs/Hallway.h>
-#include <visualization_msgs/VisualizationMarker.h>
+#include <visualization_msgs/Marker.h>
// Sample Consensus
#include <point_cloud_mapping/sample_consensus/sac.h>
@@ -99,7 +99,7 @@
ros::Node *node_;
- PointCloud cloud_;
+ PointCloud cloud_;
laser_scan::LaserProjection projector_; // Used to project laser scans into point clouds
tf::TransformListener *tf_;
@@ -120,12 +120,12 @@
HallwayTracker():message_notifier_(NULL)
{
node_ = ros::Node::instance();
- tf_ = new tf::TransformListener(*node_);
-
- // Get params from the param server.
- node_->param("~p_base_laser_topic", base_laser_topic_, string("base_scan"));
- node_->param("~p_sac_min_points_per_model", sac_min_points_per_model_, 50);
- node_->param("~p_sac_distance_threshold", sac_distance_threshold_, 0.03); // 3 cm
+ tf_ = new tf::TransformListener(*node_);
+
+ // Get params from the param server.
+ node_->param("~p_base_laser_topic", base_laser_topic_, string("base_scan"));
+ node_->param("~p_sac_min_points_per_model", sac_min_points_per_model_, 50);
+ node_->param("~p_sac_distance_threshold", sac_distance_threshold_, 0.03); // 3 cm
//node_->param("~p_eps_angle", eps_angle_, 10.0); // 10 degrees
node_->param("~p_fixed_frame", fixed_frame_, string("odom_combined"));
node_->param("~p_min_hallway_width_m", min_hallway_width_m_, 1.0);
@@ -145,9 +145,9 @@
filter_chain_.configure(1, config);
- // Visualization:
+ // Visualization:
// The visualization markers are the two lines. The start/end points are arbitrary.
- node_->advertise<visualization_msgs::VisualizationMarker>( "visualizationMarker", 0 );
+ node_->advertise<visualization_msgs::Marker>( "visualization_marker", 0 );
// A point cloud of model inliers.
node_->advertise<robot_msgs::PointCloud>("parallel_lines_inliers",10);
@@ -161,7 +161,7 @@
~HallwayTracker()
{
- node_->unadvertise("visualizationMarker");
+ node_->unadvertise("visualization_marker");
node_->unadvertise("parallel_lines_inliers");
delete message_notifier_;
}
@@ -199,7 +199,7 @@
int cloud_size = cloud_.pts.size();
vector<int> possible_hallway_points;
- possible_hallway_points.resize(cloud_size);
+ possible_hallway_points.resize(cloud_size);
// Keep only points that are within max_point_dist_m_ of the robot.
int iind = 0;
@@ -260,7 +260,7 @@
}
else {
// No parallel lines were found.
- }
+ }
}
@@ -270,25 +270,19 @@
*/
void visualization(std::vector<double> coeffs, std::vector<int> inliers) {
// First line:
- visualization_msgs::VisualizationMarker marker;
+ visualization_msgs::Marker marker;
marker.header.frame_id = fixed_frame_;
marker.header.stamp = cloud_.header.stamp;
+ marker.ns = "hallway_tracker";
marker.id = 0;
- marker.type = visualization_msgs::VisualizationMarker::LINE_STRIP;
- marker.action = visualization_msgs::VisualizationMarker::ADD;
- marker.x = 0.0;
- marker.y = 0.0;
- marker.z = 0.0;
- marker.yaw = 0.0;
- marker.pitch = 0.0;
- marker.roll = 0.0;
- marker.xScale = 0.05;//0.01;
- marker.yScale = 0.05;//0.1;
- marker.zScale = 0.05;//0.1;
- marker.alpha = 255;
- marker.r = 0;
- marker.g = 255;
- marker.b = 0;
+ marker.type = visualization_msgs::Marker::LINE_STRIP;
+ marker.action = visualization_msgs::Marker::ADD;
+ marker.pose.orientation.w = 1.0;
+ marker.scale.x = 0.05;//0.01;
+ marker.scale.y = 0.05;//0.1;
+ marker.scale.z = 0.05;//0.1;
+ marker.color.a = 1.0;
+ marker.color.g = 1.0;
marker.set_points_size(2);
robot_msgs::Point32 d;
@@ -308,7 +302,7 @@
marker.points[1].y = coeffs[1] + 6*d.y;
marker.points[1].z = coeffs[2] + 6*d.z;
- node_->publish( "visualizationMarker", marker );
+ node_->publish( "visualization_marker", marker );
// Second line:
marker.id = 1;
@@ -321,10 +315,10 @@
marker.points[1].y = coeffs[7] + 6*d.y;
marker.points[1].z = coeffs[8] + 6*d.z;
- node_->publish( "visualizationMarker", marker );
+ node_->publish( "visualization_marker", marker );
// Inlier cloud
- robot_msgs::PointCloud inlier_cloud;
+ robot_msgs::PointCloud inlier_cloud;
inlier_cloud.header.frame_id = fixed_frame_;
inlier_cloud.header.stamp = cloud_.header.stamp;
inlier_cloud.pts.resize(inliers.size());
@@ -332,7 +326,7 @@
inlier_cloud.pts[i] = cloud_.pts[inliers[i]];
}
node_->publish("parallel_lines_inliers", inlier_cloud);
- }
+ }
};
Modified: pkg/trunk/motion_planning/ompl_planning/src/display_planner_collision_model.cpp
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/src/display_planner_collision_model.cpp 2009-05-01 03:43:19 UTC (rev 14716)
+++ pkg/trunk/motion_planning/ompl_planning/src/display_planner_collision_model.cpp 2009-05-01 04:47:46 UTC (rev 14717)
@@ -1,13 +1,13 @@
/*********************************************************************
* Software License Agreement (BSD License)
-*
+*
* Copyright (c) 2008, Willow Garage, Inc.
* All rights reserved.
-*
+*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
-*
+*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
@@ -17,7 +17,7 @@
* * Neither the name of the Willow Garage nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
-*
+*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
@@ -62,7 +62,7 @@
- None
Publishes to (name/type):
-- @b "visualizationMarker"/VisualizationMarker
+- @b "visualization_marker"/visualization_msgs::Marker
<hr>
@@ -85,7 +85,7 @@
#include "kinematic_planning/CollisionSpaceMonitor.h"
#include <std_msgs/Byte.h>
-#include <visualization_msgs/VisualizationMarker.h>
+#include <visualization_msgs/Marker.h>
#include <iostream>
#include <sstream>
@@ -96,32 +96,32 @@
class DisplayPlannerCollisionModel : public CollisionSpaceMonitor
{
public:
-
+
DisplayPlannerCollisionModel(ros::Node *node) : CollisionSpaceMonitor(node)
{
id_ = 0;
-
- m_node->advertise<visualization_msgs::VisualizationMarker>("visualizationMarker", 10240);
+
+ m_node->advertise<visualization_msgs::Marker>("visualization_marker", 10240);
m_node->advertise<robot_msgs::AttachedObject>("attach_object", 1);
}
-
+
virtual ~DisplayPlannerCollisionModel(void)
{
}
-
+
void run(void)
{
loadRobotDescription();
m_node->spin();
}
-
+
protected:
-
+
void afterWorldUpdate(void)
{
CollisionSpaceMonitor::afterWorldUpdate();
-
- unsigned int n = m_collisionMap.get_boxes_size();
+
+ unsigned int n = m_collisionMap.get_boxes_size();
for (unsigned int i = 0 ; i < n ; ++i)
{
sendPoint(m_collisionMap.boxes[i].center.x,
@@ -131,7 +131,7 @@
m_collisionMap.header, 1);...
[truncated message content] |
