Menu
▾
▴
personalrobots-commit
|
From: <is...@us...> - 2009-07-31 08:45:24
|
Revision: 20254
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=20254&view=rev
Author: isucan
Date: 2009-07-31 08:45:14 +0000 (Fri, 31 Jul 2009)
Log Message:
-----------
removed some extra output
Modified Paths:
--------------
pkg/trunk/motion_planning/planning_models/src/kinematic.cpp
pkg/trunk/util/geometric_shapes/src/bodies.cpp
Modified: pkg/trunk/motion_planning/planning_models/src/kinematic.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_models/src/kinematic.cpp 2009-07-31 08:29:32 UTC (rev 20253)
+++ pkg/trunk/motion_planning/planning_models/src/kinematic.cpp 2009-07-31 08:45:14 UTC (rev 20254)
@@ -264,7 +264,7 @@
static_cast<const robot_desc::URDF::Link::Geometry::Mesh*>(urdfLink->collision->geometry->shape)->filename;
if (filename.rfind(".stl") == std::string::npos)
filename += ".stl";
- std::cout << "Loading '" << filename << "'" << std::endl;
+ // std::cout << "Loading '" << filename << "'" << std::endl;
shapes::Mesh *mesh = shapes::create_mesh_from_binary_stl(filename.c_str());
shape = mesh;
}
Modified: pkg/trunk/util/geometric_shapes/src/bodies.cpp
===================================================================
--- pkg/trunk/util/geometric_shapes/src/bodies.cpp 2009-07-31 08:29:32 UTC (rev 20253)
+++ pkg/trunk/util/geometric_shapes/src/bodies.cpp 2009-07-31 08:45:14 UTC (rev 20254)
@@ -491,7 +491,7 @@
HullLibrary hl;
if (hl.CreateConvexHull(hd, hr) == QE_OK)
{
- std::cout << "Convex hull has " << hr.m_OutputVertices.size() << " vertices (down from " << mesh->vertexCount << "), " << hr.mNumFaces << " faces" << std::endl;
+ // std::cout << "Convex hull has " << hr.m_OutputVertices.size() << " vertices (down from " << mesh->vertexCount << "), " << hr.mNumFaces << " faces" << std::endl;
m_vertices.reserve(hr.m_OutputVertices.size());
btVector3 sum(0, 0, 0);
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <is...@us...> - 2009-07-31 20:24:59
|
Revision: 20307
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=20307&view=rev
Author: isucan
Date: 2009-07-31 20:24:46 +0000 (Fri, 31 Jul 2009)
Log Message:
-----------
simple script for moving an object
Modified Paths:
--------------
pkg/trunk/demos/tabletop_manipulation/tabletop_scripts/src/simple.cpp
pkg/trunk/highlevel/move_arm/src/actuate_gripper_action.cpp
pkg/trunk/highlevel/move_arm/src/arm_cmd_line.cpp
pkg/trunk/sandbox/3dnav_pr2/params/collision/collision_checks_both_arms.yaml
Modified: pkg/trunk/demos/tabletop_manipulation/tabletop_scripts/src/simple.cpp
===================================================================
--- pkg/trunk/demos/tabletop_manipulation/tabletop_scripts/src/simple.cpp 2009-07-31 20:20:06 UTC (rev 20306)
+++ pkg/trunk/demos/tabletop_manipulation/tabletop_scripts/src/simple.cpp 2009-07-31 20:24:46 UTC (rev 20307)
@@ -43,35 +43,214 @@
#include <pr2_robot_actions/MoveArmGoal.h>
#include <pr2_robot_actions/MoveArmState.h>
+#include <pr2_robot_actions/ActuateGripperState.h>
+#include <std_msgs/Float64.h>
#include <boost/thread/thread.hpp>
#include <recognition_lambertian/FindObjectPoses.h>
#include <visualization_msgs/Marker.h>
-void sendPoint(ros::Publisher &vmPub, const roslib::Header &header, double x, double y, double z)
+class CleanTable
{
- visualization_msgs::Marker mk;
+
+public:
- mk.header = header;
- mk.ns = "test_recognition";
- mk.id = 1;
- mk.type = visualization_msgs::Marker::SPHERE;
- mk.action = visualization_msgs::Marker::ADD;
- mk.pose.position.x = x;
- mk.pose.position.y = y;
- mk.pose.position.z = z;
- mk.pose.orientation.w = 1.0;
+ CleanTable(void) : move_arm("move_right_arm"), gripper("actuate_gripper_right_arm")
+ {
+ pubObj = nh.advertise<mapping_msgs::ObjectInMap>("object_in_map", 1);
+ pubAttach = nh.advertise<mapping_msgs::AttachedObject>("attach_map", 1);
+
+ vmPub = nh.advertise<visualization_msgs::Marker>("visualization_marker", 10240);
+ }
- mk.scale.x = mk.scale.y = mk.scale.z = 0.03;
+ void sendPoint(const roslib::Header &header, double x, double y, double z)
+ {
+ visualization_msgs::Marker mk;
+
+ mk.header = header;
+ mk.ns = "test_recognition";
+ mk.id = 1;
+ mk.type = visualization_msgs::Marker::SPHERE;
+ mk.action = visualization_msgs::Marker::ADD;
+ mk.pose.position.x = x;
+ mk.pose.position.y = y;
+ mk.pose.position.z = z;
+ mk.pose.orientation.w = 1.0;
+
+ mk.scale.x = mk.scale.y = mk.scale.z = 0.03;
+
+ mk.color.a = 1.0;
+ mk.color.r = 0.5;
+ mk.color.g = 0.4;
+ mk.color.b = 0.04;
+
+ vmPub.publish(mk);
+ }
- mk.color.a = 1.0;
- mk.color.r = 0.5;
- mk.color.g = 0.4;
- mk.color.b = 0.04;
+ bool findObject(recognition_lambertian::TableTopObject &obj)
+ {
+ recognition_lambertian::FindObjectPoses::Request req;
+ recognition_lambertian::FindObjectPoses::Response res;
+
+ ros::ServiceClient client = nh.serviceClient<recognition_lambertian::FindObjectPoses>("table_top/find_object_poses");
+ if (client.call(req, res))
+ {
+ ROS_INFO("Found %d objects", (int)res.objects.size());
+ if (res.objects.empty())
+ return false;
+
+ obj = res.objects[0];
+ for (unsigned int i = 1 ; i < res.objects.size() ; ++i)
+ {
+ if (res.objects[i].grasp_pose.pose.position.y < obj.grasp_pose.pose.position.y)
+ obj = res.objects[i];
+ }
+
+ // hack for stereo error
+ double dx = 0.055;
+ double dy = 0.02;
+ double dz = -0.04;
+
+ obj.object_pose.pose.position.x += dx;
+ obj.object_pose.pose.position.y += dy;
+ obj.object_pose.pose.position.z += dz;
+ obj.object_pose.pose.orientation.x = 0;
+ obj.object_pose.pose.orientation.y = 0;
+ obj.object_pose.pose.orientation.z = 0;
+ obj.object_pose.pose.orientation.w = 1;
+
+
+ obj.grasp_pose.pose.position.x += dx;
+ obj.grasp_pose.pose.position.y += dy;
+ obj.grasp_pose.pose.position.z += dz;
+ obj.grasp_pose.pose.orientation.x = 0;
+ obj.grasp_pose.pose.orientation.y = 0;
+ obj.grasp_pose.pose.orientation.z = 0;
+ obj.grasp_pose.pose.orientation.w = 1;
+
+ obj.grasp_pose.pose.position.x -= 0.16;
+
+ ROS_INFO("pose of object: %f %f %f", obj.object_pose.pose.position.x, obj.object_pose.pose.position.y, obj.object_pose.pose.position.z);
+ ROS_INFO("point to grasp: %f %f %f", obj.grasp_pose.pose.position.x, obj.grasp_pose.pose.position.y, obj.grasp_pose.pose.position.z);
+
+ return true;
+ }
+ else
+ {
+ ROS_ERROR("Unable to call service for finding object");
+ return false;
+ }
+ }
- vmPub.publish(mk);
-}
+ bool moveTo(recognition_lambertian::TableTopObject &obj)
+ {
+ pr2_robot_actions::MoveArmGoal goal;
+ int32_t feedback;
+
+ goal.goal_constraints.set_pose_constraint_size(1);
+ goal.goal_constraints.pose_constraint[0].pose = obj.grasp_pose;
+ goal.goal_constraints.pose_constraint[0].link_name = "r_wrist_roll_link";
+
+ goal.goal_constraints.pose_constraint[0].position_tolerance_above.x = 0.005;
+ goal.goal_constraints.pose_constraint[0].position_tolerance_below.x = 0.005;
+
+ goal.goal_constraints.pose_constraint[0].position_tolerance_above.y = 0.01;
+ goal.goal_constraints.pose_constraint[0].position_tolerance_below.y = 0.01;
+
+ goal.goal_constraints.pose_constraint[0].position_tolerance_above.z = 0.02;
+ goal.goal_constraints.pose_constraint[0].position_tolerance_below.z = 0.02;
+
+ goal.goal_constraints.pose_constraint[0].orientation_tolerance_above.x = 0.1;
+ goal.goal_constraints.pose_constraint[0].orientation_tolerance_below.x = 0.1;
+ goal.goal_constraints.pose_constraint[0].orientation_tolerance_above.y = 0.1;
+ goal.goal_constraints.pose_constraint[0].orientation_tolerance_below.y = 0.1;
+ goal.goal_constraints.pose_constraint[0].orientation_tolerance_above.z = 0.1;
+ goal.goal_constraints.pose_constraint[0].orientation_tolerance_below.z = 0.1;
+
+ goal.goal_constraints.pose_constraint[0].orientation_importance = 0.1;
+ goal.goal_constraints.pose_constraint[0].type =
+ motion_planning_msgs::PoseConstraint::POSITION_X + motion_planning_msgs::PoseConstraint::POSITION_Y + motion_planning_msgs::PoseConstraint::POSITION_Z +
+ motion_planning_msgs::PoseConstraint::ORIENTATION_R + motion_planning_msgs::PoseConstraint::ORIENTATION_P + motion_planning_msgs::PoseConstraint::ORIENTATION_Y;
+
+ sendPoint(obj.grasp_pose.header, obj.grasp_pose.pose.position.x, obj.grasp_pose.pose.position.y, obj.grasp_pose.pose.position.z);
+
+ if (move_arm.execute(goal, feedback, ros::Duration(60.0)) != robot_actions::SUCCESS)
+ {
+ ROS_ERROR("Failed achieving goal");
+ return false;
+ }
+ else
+ return true;
+ }
+ bool gripClose(void)
+ {
+ std_msgs::Float64 g, fb;
+ g.data = -80;
+ if (gripper.execute(g, fb, ros::Duration(5.0)) != robot_actions::SUCCESS)
+ return true;
+ else
+ {
+ ROS_ERROR("Gripper close failed");
+ return false;
+ }
+ }
+
+ bool gripOpen(void)
+ {
+ std_msgs::Float64 g, fb;
+ g.data = 80;
+ if (gripper.execute(g, fb, ros::Duration(5.0)) != robot_actions::SUCCESS)
+ return true;
+ else
+ {
+ ROS_ERROR("Gripper open failed");
+ return false;
+ }
+ }
+
+ bool graspObject(recognition_lambertian::TableTopObject &obj)
+ {
+ if (gripClose())
+ {
+ mapping_msgs::AttachedObject ao;
+ ao.header = obj.object_pose.header;
+ ao.link_name = "r_wrist_roll_link";
+ ao.objects.push_back(obj.object);
+ ao.poses.push_back(obj.object_pose.pose);
+ pubAttach.publish(ao);
+ return true;
+ }
+ else return false;
+ }
+
+ bool releaseObject(void)
+ {
+ if (gripOpen())
+ {
+ mapping_msgs::AttachedObject ao;
+ ao.header.stamp = ros::Time::now();
+ ao.header.frame_id = "r_wrist_roll_link";
+ ao.link_name = "r_wrist_roll_link";
+ pubAttach.publish(ao);
+ return true;
+ }
+ else return false;
+ }
+
+private:
+
+ robot_actions::ActionClient<pr2_robot_actions::MoveArmGoal, pr2_robot_actions::MoveArmState, int32_t> move_arm;
+ robot_actions::ActionClient<std_msgs::Float64, pr2_robot_actions::ActuateGripperState, std_msgs::Float64> gripper;
+
+ ros::NodeHandle nh;
+ ros::Publisher pubObj;
+ ros::Publisher pubAttach;
+ ros::Publisher vmPub;
+
+};
+
+
void spinThread(void)
{
ros::spin();
@@ -80,14 +259,28 @@
int main(int argc, char **argv)
{
ros::init(argc, argv, "test_recognition_planning");
- ros::NodeHandle nh;
- sleep(1);
-
boost::thread th(&spinThread);
- ros::Publisher pubObj = nh.advertise<mapping_msgs::ObjectInMap>("object_in_map", 1);
- ros::Publisher pubAttach = nh.advertise<mapping_msgs::AttachedObject>("attach_map", 1);
+ CleanTable ct;
+ recognition_lambertian::TableTopObject obj;
+
+ if (ct.findObject(obj))
+ {
+ if (ct.moveTo(obj))
+ {
+ ct.graspObject(obj);
+ recognition_lambertian::TableTopObject obj2 = obj;
+ obj2.grasp_pose.pose.position.y -= 0.25;
+ if (ct.moveTo(obj2))
+ {
+ ct.releaseObject();
+ }
+ }
+ }
+
+
+
/*
mapping_msgs::ObjectInMap o1;
o1.header.frame_id = "/base_link";
@@ -136,107 +329,7 @@
return 0;
*/
-
- recognition_lambertian::FindObjectPoses::Request req;
- recognition_lambertian::FindObjectPoses::Response res;
- ros::Publisher vmPub = nh.advertise<visualization_msgs::Marker>("visualization_marker", 10240);
- robot_actions::ActionClient<pr2_robot_actions::MoveArmGoal, pr2_robot_actions::MoveArmState, int32_t> move_arm("move_right_arm");
-
- ros::ServiceClient client = nh.serviceClient<recognition_lambertian::FindObjectPoses>("table_top/find_object_poses");
- if (client.call(req, res))
- {
- ROS_INFO("Found %d objects", (int)res.objects.size());
- if (res.objects.size() > 0)
- {
- recognition_lambertian::TableTopObject obj = res.objects[0];
-
- double dx = 0.055;
- double dy = 0.02;
- double dz = -0.04;
-
- obj.object_pose.pose.position.x += dx;
- obj.object_pose.pose.position.y += dy;
- obj.object_pose.pose.position.z += dz;
- obj.object_pose.pose.orientation.x = 0;
- obj.object_pose.pose.orientation.y = 0;
- obj.object_pose.pose.orientation.z = 0;
- obj.object_pose.pose.orientation.w = 1;
-
-
- obj.grasp_pose.pose.position.x += dx;
- obj.grasp_pose.pose.position.y += dy;
- obj.grasp_pose.pose.position.z += dz;
- obj.grasp_pose.pose.orientation.x = 0;
- obj.grasp_pose.pose.orientation.y = 0;
- obj.grasp_pose.pose.orientation.z = 0;
- obj.grasp_pose.pose.orientation.w = 1;
-
-
-
- obj.grasp_pose.pose.position.x -= 0.16;
-
- ROS_INFO("pose of object: %f %f %f", obj.object_pose.pose.position.x, obj.object_pose.pose.position.y, obj.object_pose.pose.position.z);
- ROS_INFO("point to grasp: %f %f %f", obj.grasp_pose.pose.position.x, obj.grasp_pose.pose.position.y, obj.grasp_pose.pose.position.z);
-
- mapping_msgs::ObjectInMap o1;
- o1.header = obj.object_pose.header;
- o1.id = "Part1";
- o1.action = mapping_msgs::ObjectInMap::ADD;
- o1.object = obj.object;
- o1.pose = obj.object_pose.pose;
- pubObj.publish(o1);
-
-
-
- int32_t feedback;
- pr2_robot_actions::MoveArmGoal goal;
-
- goal.goal_constraints.set_pose_constraint_size(1);
- goal.goal_constraints.pose_constraint[0].pose = obj.grasp_pose;
- goal.goal_constraints.pose_constraint[0].link_name = "r_wrist_roll_link";
-
- goal.goal_constraints.pose_constraint[0].position_tolerance_above.x = 0.02;
- goal.goal_constraints.pose_constraint[0].position_tolerance_below.x = 0.02;
-
- goal.goal_constraints.pose_constraint[0].position_tolerance_above.y = 0.01;
- goal.goal_constraints.pose_constraint[0].position_tolerance_below.y = 0.01;
-
- goal.goal_constraints.pose_constraint[0].position_tolerance_above.z = 0.02;
- goal.goal_constraints.pose_constraint[0].position_tolerance_below.z = 0.02;
-
- goal.goal_constraints.pose_constraint[0].orientation_tolerance_above.x = 0.01;
- goal.goal_constraints.pose_constraint[0].orientation_tolerance_below.x = 0.01;
- goal.goal_constraints.pose_constraint[0].orientation_tolerance_above.y = 0.01;
- goal.goal_constraints.pose_constraint[0].orientation_tolerance_below.y = 0.01;
- goal.goal_constraints.pose_constraint[0].orientation_tolerance_above.z = 0.01;
- goal.goal_constraints.pose_constraint[0].orientation_tolerance_below.z = 0.01;
-
- goal.goal_constraints.pose_constraint[0].orientation_importance = 0.1;
- goal.goal_constraints.pose_constraint[0].type =
- motion_planning_msgs::PoseConstraint::POSITION_X + motion_planning_msgs::PoseConstraint::POSITION_Y + motion_planning_msgs::PoseConstraint::POSITION_Z +
- motion_planning_msgs::PoseConstraint::ORIENTATION_R + motion_planning_msgs::PoseConstraint::ORIENTATION_P + motion_planning_msgs::PoseConstraint::ORIENTATION_Y;
-
- sendPoint(vmPub, obj.grasp_pose.header, obj.grasp_pose.pose.position.x, obj.grasp_pose.pose.position.y, obj.grasp_pose.pose.position.z);
-
- /*
- if (move_arm.execute(goal, feedback, ros::Duration(60.0)) != robot_actions::SUCCESS)
- ROS_ERROR("failed achieving goal");
- else
- {
- mapping_msgs::AttachedObject ao;
- ao.header = obj.object_pose.header;
- ao.link_name = "r_wrist_roll_link";
- ao.objects.push_back(obj.object);
- ao.poses.push_back(obj.object_pose.pose);
- pubAttach.publish(ao);
- }
- */
- }
- }
- else
- ROS_ERROR("Unable to call find_object_poses service");
-
ROS_INFO("Done");
th.join();
Modified: pkg/trunk/highlevel/move_arm/src/actuate_gripper_action.cpp
===================================================================
--- pkg/trunk/highlevel/move_arm/src/actuate_gripper_action.cpp 2009-07-31 20:20:06 UTC (rev 20306)
+++ pkg/trunk/highlevel/move_arm/src/actuate_gripper_action.cpp 2009-07-31 20:24:46 UTC (rev 20307)
@@ -70,7 +70,7 @@
ros::Rate r(10.0);
ros::Time start = ros::Time::now();
- while(ros::Time::now()-start < ros::Duration(10.0))
+ while(ros::Time::now()-start < ros::Duration(4.0))
{
if (isPreemptRequested()) {
gripper_msg.data = 0.0;
Modified: pkg/trunk/highlevel/move_arm/src/arm_cmd_line.cpp
===================================================================
--- pkg/trunk/highlevel/move_arm/src/arm_cmd_line.cpp 2009-07-31 20:20:06 UTC (rev 20306)
+++ pkg/trunk/highlevel/move_arm/src/arm_cmd_line.cpp 2009-07-31 20:24:46 UTC (rev 20307)
@@ -170,12 +170,12 @@
goal.goal_constraints.pose_constraint[0].position_tolerance_below.y = 0.01;
goal.goal_constraints.pose_constraint[0].position_tolerance_below.z = 0.015;
- goal.goal_constraints.pose_constraint[0].orientation_tolerance_above.x = 0.15;
- goal.goal_constraints.pose_constraint[0].orientation_tolerance_above.y = 0.15;
- goal.goal_constraints.pose_constraint[0].orientation_tolerance_above.z = 0.15;
- goal.goal_constraints.pose_constraint[0].orientation_tolerance_below.x = 0.15;
- goal.goal_constraints.pose_constraint[0].orientation_tolerance_below.y = 0.15;
- goal.goal_constraints.pose_constraint[0].orientation_tolerance_below.z = 0.15;
+ goal.goal_constraints.pose_constraint[0].orientation_tolerance_above.x = 0.05;
+ goal.goal_constraints.pose_constraint[0].orientation_tolerance_above.y = 0.05;
+ goal.goal_constraints.pose_constraint[0].orientation_tolerance_above.z = 0.05;
+ goal.goal_constraints.pose_constraint[0].orientation_tolerance_below.x = 0.05;
+ goal.goal_constraints.pose_constraint[0].orientation_tolerance_below.y = 0.05;
+ goal.goal_constraints.pose_constraint[0].orientation_tolerance_below.z = 0.05;
goal.goal_constraints.pose_constraint[0].orientation_importance = 0.01;
}
Modified: pkg/trunk/sandbox/3dnav_pr2/params/collision/collision_checks_both_arms.yaml
===================================================================
--- pkg/trunk/sandbox/3dnav_pr2/params/collision/collision_checks_both_arms.yaml 2009-07-31 20:20:06 UTC (rev 20306)
+++ pkg/trunk/sandbox/3dnav_pr2/params/collision/collision_checks_both_arms.yaml 2009-07-31 20:24:46 UTC (rev 20307)
@@ -90,7 +90,7 @@
## The padding to be added for the body parts the robot can see
-self_see_padd: 0.05
+self_see_padd: 0.002
## The scaling to be considered for the body parts the robot can see
self_see_scale: 1.0
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <pmi...@us...> - 2009-07-31 22:17:25
|
Revision: 20324
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=20324&view=rev
Author: pmihelich
Date: 2009-07-31 22:17:14 +0000 (Fri, 31 Jul 2009)
Log Message:
-----------
Renamed CamInfo message to CameraInfo.
Modified Paths:
--------------
pkg/trunk/calibration/kinematic_calibration/deprecated/camera_cal_sampler.h
pkg/trunk/calibration/kinematic_calibration/msg/CalibrationData2.msg
pkg/trunk/calibration/kinematic_calibration/msg/SensorKinematics.msg
pkg/trunk/calibration/kinematic_calibration/src/laser_head_grabber.cpp
pkg/trunk/calibration/kinematic_calibration/src/sensor_kinematics_grabber.cpp
pkg/trunk/drivers/cam/forearm_cam/src/nodes/forearm_node.cpp
pkg/trunk/drivers/cam/forearm_cam/src/nodes/forearm_node_new.cpp
pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/ros_prosilica.h
pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/ros_stereo_camera.h
pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_prosilica.cpp
pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_stereo_camera.cpp
pkg/trunk/mapping/annotated_planar_patch_map/include/annotated_planar_patch_map/projection.h
pkg/trunk/mapping/annotated_planar_patch_map/src/annotation2d_lifter_to_pcd_via_service.cpp
pkg/trunk/mapping/annotated_planar_patch_map/src/annotation2d_lifter_to_tagged_patch_map.cpp
pkg/trunk/mapping/annotated_planar_patch_map/src/annotation2d_lifter_to_tagged_patch_map_via_service.cpp
pkg/trunk/mapping/annotated_planar_patch_map/src/image_value_node.cpp
pkg/trunk/mapping/annotated_planar_patch_map/src/projection.cpp
pkg/trunk/mapping/door_stereo_detector/src/door_stereo_detector.cpp
pkg/trunk/openrave_planning/openraveros/src/rosserver.h
pkg/trunk/openrave_planning/openraveros/srv/robot_sensorgetdata.srv
pkg/trunk/sandbox/camera_calibration/include/camera_calibration/pinhole.h
pkg/trunk/sandbox/camera_calibration/src/library/pinhole.cpp
pkg/trunk/sandbox/mturk_grab_object/include/mturk_grab_object/grasp_table_object_node.h
pkg/trunk/sandbox/planar_objects/src/box_detector.cpp
pkg/trunk/sandbox/planar_objects/src/box_detector.h
pkg/trunk/sandbox/planar_objects/src/cornercandidate.h
pkg/trunk/sandbox/rf_detector/include/ObjectInPerspective.h
pkg/trunk/sandbox/rf_detector/src/ObjectInPerspective.cpp
pkg/trunk/sandbox/rf_detector/src/RFDetector.cpp
pkg/trunk/stacks/camera_drivers/dcam/src/nodes/dcam.cpp
pkg/trunk/stacks/camera_drivers/prosilica_cam/src/nodes/fake_poll_node.cpp
pkg/trunk/stacks/camera_drivers/prosilica_cam/src/nodes/prosilica_node.cpp
pkg/trunk/stacks/camera_drivers/prosilica_cam/srv/PolledImage.srv
pkg/trunk/stacks/common_msgs/sensor_msgs/msg/RawStereo.msg
pkg/trunk/stacks/semantic_mapping/door_handle_detector/src/handle_detector_vision.cpp
pkg/trunk/stacks/stereo/stereo_image_proc/include/cam_bridge.h
pkg/trunk/stacks/stereo/stereo_image_proc/src/imageproc.cpp
pkg/trunk/stacks/stereo/stereo_image_proc/src/stereoproc.cpp
pkg/trunk/stacks/visual_feature_detectors/checkerboard_detector/checkerboard_detector.cpp
pkg/trunk/stacks/visual_feature_detectors/outlet_detection/include/outlet_detection/tracker_base.h
pkg/trunk/stacks/visual_feature_detectors/outlet_detection/src/outlet_spotting.cpp
pkg/trunk/stacks/visual_feature_detectors/outlet_detection/src/outlet_spotting2.cpp
pkg/trunk/stacks/visual_feature_detectors/outlet_detection/src/tracker_base.cpp
pkg/trunk/stacks/visualization/rviz/src/rviz/displays/camera_display.cpp
pkg/trunk/stacks/visualization/rviz/src/rviz/displays/camera_display.h
pkg/trunk/vision/led_detection/include/led_detection/led_detector.h
pkg/trunk/vision/led_detection/include/led_detection/led_detector_node.h
pkg/trunk/vision/led_detection/src/led_detector.cpp
pkg/trunk/vision/people/src/face_detection.cpp
pkg/trunk/vision/people/src/pedestrian_detector_HOG.cpp
pkg/trunk/vision/people/src/stereo_color_tracker.cpp
pkg/trunk/vision/people/src/stereo_face_color_tracker.cpp
pkg/trunk/vision/people/src/track_starter_gui.cpp
pkg/trunk/vision/recognition_lambertian/src/publish_scene.cpp
pkg/trunk/vision/recognition_lambertian/src/rec_lam_normal_features.cpp
pkg/trunk/vision/recognition_lambertian/src/recognition_lambertian.cpp
pkg/trunk/vision/stereo_capture/src/periodic_capture.cpp
pkg/trunk/vision/stereo_capture/src/stereo_capture.cpp
pkg/trunk/vision/stereo_checkerboard_detector/include/stereo_checkerboard_detector/reprojection_helper.h
pkg/trunk/vision/stereo_checkerboard_detector/include/stereo_checkerboard_detector/stereo_checkerboard_helper.h
pkg/trunk/vision/stereo_checkerboard_detector/src/image_pc_debugger.cpp
pkg/trunk/vision/stereo_checkerboard_detector/src/reprojection_helper.cpp
pkg/trunk/vision/stereo_checkerboard_detector/src/stereo_checkerboard_helper.cpp
pkg/trunk/vision/stereo_checkerboard_detector/src/stereo_checkerboard_node.cpp
Added Paths:
-----------
pkg/trunk/stacks/camera_drivers/prosilica_cam/srv/CameraInfo.srv
pkg/trunk/stacks/common_msgs/sensor_msgs/msg/CameraInfo.msg
Removed Paths:
-------------
pkg/trunk/stacks/camera_drivers/prosilica_cam/srv/CamInfo.srv
pkg/trunk/stacks/common_msgs/sensor_msgs/msg/CamInfo.msg
Modified: pkg/trunk/calibration/kinematic_calibration/deprecated/camera_cal_sampler.h
===================================================================
--- pkg/trunk/calibration/kinematic_calibration/deprecated/camera_cal_sampler.h 2009-07-31 22:10:05 UTC (rev 20323)
+++ pkg/trunk/calibration/kinematic_calibration/deprecated/camera_cal_sampler.h 2009-07-31 22:17:14 UTC (rev 20324)
@@ -46,7 +46,7 @@
#include "kinematic_calibration/Capture.h"
#include "robot_msgs/MechanismState.h"
#include "sensor_msgs/Image.h"
-#include "sensor_msgs/CamInfo.h"
+#include "sensor_msgs/CameraInfo.h"
#include "topic_synchronizer/topic_synchronizer.h"
#include "tinyxml/tinyxml.h"
Modified: pkg/trunk/calibration/kinematic_calibration/msg/CalibrationData2.msg
===================================================================
--- pkg/trunk/calibration/kinematic_calibration/msg/CalibrationData2.msg 2009-07-31 22:10:05 UTC (rev 20323)
+++ pkg/trunk/calibration/kinematic_calibration/msg/CalibrationData2.msg 2009-07-31 22:17:14 UTC (rev 20324)
@@ -18,4 +18,4 @@
# Data from multiple monocular cameras
sensor_msgs/Image[] image
-sensor_msgs/CamInfo[] cam_info
+sensor_msgs/CameraInfo[] cam_info
Modified: pkg/trunk/calibration/kinematic_calibration/msg/SensorKinematics.msg
===================================================================
--- pkg/trunk/calibration/kinematic_calibration/msg/SensorKinematics.msg 2009-07-31 22:10:05 UTC (rev 20323)
+++ pkg/trunk/calibration/kinematic_calibration/msg/SensorKinematics.msg 2009-07-31 22:17:14 UTC (rev 20324)
@@ -9,8 +9,8 @@
sensor_msgs/Image right_image
sensor_msgs/Image disparity_image
sensor_msgs/StereoInfo stereo_info
-sensor_msgs/CamInfo left_info
-sensor_msgs/CamInfo right_info
+sensor_msgs/CameraInfo left_info
+sensor_msgs/CameraInfo right_info
# Mechanism State - Stores the robots joint angles
mechanism_msgs/MechanismState mechanism_state
Modified: pkg/trunk/calibration/kinematic_calibration/src/laser_head_grabber.cpp
===================================================================
--- pkg/trunk/calibration/kinematic_calibration/src/laser_head_grabber.cpp 2009-07-31 22:10:05 UTC (rev 20323)
+++ pkg/trunk/calibration/kinematic_calibration/src/laser_head_grabber.cpp 2009-07-31 22:17:14 UTC (rev 20324)
@@ -44,7 +44,7 @@
#include "robot_msgs/PointCloud.h"
#include "sensor_msgs/Image.h"
-#include "sensor_msgs/CamInfo.h"
+#include "sensor_msgs/CameraInfo.h"
#include "kinematic_calibration/CalSnapshot.h"
@@ -90,8 +90,8 @@
ADD_MSG(robot_msgs::PointCloud, corners_right_) ;
ADD_MSG(sensor_msgs::Image, left_debug_) ;
ADD_MSG(sensor_msgs::Image, right_debug_) ;
- ADD_MSG(sensor_msgs::CamInfo, left_info_) ;
- ADD_MSG(sensor_msgs::CamInfo, right_info_) ;
+ ADD_MSG(sensor_msgs::CameraInfo, left_info_) ;
+ ADD_MSG(sensor_msgs::CameraInfo, right_info_) ;
// Laser Messages
boost::mutex laser_lock_ ;
@@ -333,8 +333,8 @@
SensorSample left_cb_sample = buildCamCornersSample(safe_corners_left_, "stereo_left", "6x8_cb") ;
SensorSample right_cb_sample = buildCamCornersSample(safe_corners_right_, "stereo_right", "6x8_cb") ;
- SensorSample left_info_sample = buildCamInfoSample(safe_left_info_, "stereo_left_info","6x8_cb") ;
- SensorSample right_info_sample = buildCamInfoSample(safe_right_info_, "stereo_right_info","6x8_cb") ;
+ SensorSample left_info_sample = buildCameraInfoSample(safe_left_info_, "stereo_left_info","6x8_cb") ;
+ SensorSample right_info_sample = buildCameraInfoSample(safe_right_info_, "stereo_right_info","6x8_cb") ;
snapshot_msg_.samples.resize(5) ;
snapshot_msg_.samples[0] = laser_sample ;
@@ -391,7 +391,7 @@
return sample ;
}
- SensorSample buildCamInfoSample(const sensor_msgs::CamInfo& info, const string& sensor, const string& target)
+ SensorSample buildCameraInfoSample(const sensor_msgs::CameraInfo& info, const string& sensor, const string& target)
{
SensorSample sample ;
sample.sensor = sensor ;
Modified: pkg/trunk/calibration/kinematic_calibration/src/sensor_kinematics_grabber.cpp
===================================================================
--- pkg/trunk/calibration/kinematic_calibration/src/sensor_kinematics_grabber.cpp 2009-07-31 22:10:05 UTC (rev 20323)
+++ pkg/trunk/calibration/kinematic_calibration/src/sensor_kinematics_grabber.cpp 2009-07-31 22:17:14 UTC (rev 20324)
@@ -46,7 +46,7 @@
#include "sensor_msgs/RawStereo.h"
#include "sensor_msgs/Image.h"
-#include "sensor_msgs/CamInfo.h"
+#include "sensor_msgs/CameraInfo.h"
#include "kinematic_calibration/CalibrationData2.h"
@@ -94,8 +94,8 @@
// HiRes Camera messages
sensor_msgs::Image hi_res_image_ ;
sensor_msgs::Image safe_hi_res_image_ ;
- sensor_msgs::CamInfo hi_res_info_ ;
- sensor_msgs::CamInfo safe_hi_res_info_ ;
+ sensor_msgs::CameraInfo hi_res_info_ ;
+ sensor_msgs::CameraInfo safe_hi_res_info_ ;
boost::mutex hi_res_lock_ ;
// Point Cloud Messages
Modified: pkg/trunk/drivers/cam/forearm_cam/src/nodes/forearm_node.cpp
===================================================================
--- pkg/trunk/drivers/cam/forearm_cam/src/nodes/forearm_node.cpp 2009-07-31 22:10:05 UTC (rev 20323)
+++ pkg/trunk/drivers/cam/forearm_cam/src/nodes/forearm_node.cpp 2009-07-31 22:17:14 UTC (rev 20324)
@@ -42,7 +42,7 @@
#include <ros/node.h>
#include <sensor_msgs/Image.h>
-#include <sensor_msgs/CamInfo.h>
+#include <sensor_msgs/CameraInfo.h>
#include <sensor_msgs/FillImage.h>
#include <dynamic_reconfigure/SensorLevels.h>
#include <diagnostic_updater/diagnostic_updater.h>
@@ -192,9 +192,9 @@
// Publications
diagnostic_updater::DiagnosedPublisher<sensor_msgs::Image> cam_pub_;
- diagnostic_updater::DiagnosedPublisher<sensor_msgs::CamInfo> cam_info_pub_;
+ diagnostic_updater::DiagnosedPublisher<sensor_msgs::CameraInfo> cam_info_pub_;
sensor_msgs::Image image_;
- sensor_msgs::CamInfo cam_info_;
+ sensor_msgs::CameraInfo cam_info_;
// Services
@@ -633,7 +633,7 @@
// Receive frames through callback
if (calibrated_)
- cam_info_pub_.set_publisher(node_handle_.advertise<sensor_msgs::CamInfo>("~cam_info", 1));
+ cam_info_pub_.set_publisher(node_handle_.advertise<sensor_msgs::CameraInfo>("~cam_info", 1));
else
cam_info_pub_.set_publisher(ros::Publisher());
Modified: pkg/trunk/drivers/cam/forearm_cam/src/nodes/forearm_node_new.cpp
===================================================================
--- pkg/trunk/drivers/cam/forearm_cam/src/nodes/forearm_node_new.cpp 2009-07-31 22:10:05 UTC (rev 20323)
+++ pkg/trunk/drivers/cam/forearm_cam/src/nodes/forearm_node_new.cpp 2009-07-31 22:17:14 UTC (rev 20324)
@@ -42,7 +42,7 @@
#include <ros/node.h>
#include <sensor_msgs/Image.h>
-#include <sensor_msgs/CamInfo.h>
+#include <sensor_msgs/CameraInfo.h>
#include <sensor_msgs/FillImage.h>
#include <dynamic_reconfigure/SensorLevels.h>
#include <diagnostic_updater/diagnostic_updater.h>
@@ -192,9 +192,9 @@
// Publications
diagnostic_updater::DiagnosedPublisher<sensor_msgs::Image> cam_pub_;
- diagnostic_updater::DiagnosedPublisher<sensor_msgs::CamInfo> cam_info_pub_;
+ diagnostic_updater::DiagnosedPublisher<sensor_msgs::CameraInfo> cam_info_pub_;
sensor_msgs::Image image_;
- sensor_msgs::CamInfo cam_info_;
+ sensor_msgs::CameraInfo cam_info_;
// Services
@@ -633,7 +633,7 @@
// Receive frames through callback
if (calibrated_)
- cam_info_pub_.set_publisher(node_handle_.advertise<sensor_msgs::CamInfo>("~cam_info", 1));
+ cam_info_pub_.set_publisher(node_handle_.advertise<sensor_msgs::CameraInfo>("~cam_info", 1));
else
cam_info_pub_.set_publisher(ros::Publisher());
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/ros_prosilica.h
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/ros_prosilica.h 2009-07-31 22:10:05 UTC (rev 20323)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/ros_prosilica.h 2009-07-31 22:17:14 UTC (rev 20324)
@@ -35,11 +35,11 @@
// image components
#include "opencv_latest/CvBridge.h"
#include "sensor_msgs/Image.h"
-#include "sensor_msgs/CamInfo.h"
+#include "sensor_msgs/CameraInfo.h"
// prosilica components
#include "prosilica/prosilica.h"
-#include "prosilica_cam/CamInfo.h"
+#include "prosilica_cam/CameraInfo.h"
#include "prosilica_cam/PolledImage.h"
@@ -128,8 +128,8 @@
private: static void mouse_cb(int event, int x, int y, int flags, void* param) { };
/// \brief Service call to publish images, cam info
- private: bool camInfoService(prosilica_cam::CamInfo::Request &req,
- prosilica_cam::CamInfo::Response &res);
+ private: bool camInfoService(prosilica_cam::CameraInfo::Request &req,
+ prosilica_cam::CameraInfo::Response &res);
private: bool triggeredGrab(prosilica_cam::PolledImage::Request &req,
prosilica_cam::PolledImage::Response &res);
@@ -146,8 +146,8 @@
private: sensor_msgs::Image imageMsg;
private: sensor_msgs::Image imageRectMsg;
private: sensor_msgs::Image *roiImageMsg;
- private: sensor_msgs::CamInfo *camInfoMsg;
- private: sensor_msgs::CamInfo *roiCamInfoMsg;
+ private: sensor_msgs::CameraInfo *camInfoMsg;
+ private: sensor_msgs::CameraInfo *roiCameraInfoMsg;
/// \brief Parameters
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/ros_stereo_camera.h
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/ros_stereo_camera.h 2009-07-31 22:10:05 UTC (rev 20323)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/ros_stereo_camera.h 2009-07-31 22:17:14 UTC (rev 20324)
@@ -40,7 +40,7 @@
#include <sensor_msgs/Image.h>
#include "sensor_msgs/RawStereo.h"
#include "sensor_msgs/StereoInfo.h"
-#include "sensor_msgs/CamInfo.h"
+#include "sensor_msgs/CameraInfo.h"
#include "sensor_msgs/DisparityInfo.h"
namespace gazebo
@@ -167,8 +167,8 @@
private: sensor_msgs::RawStereo rawStereoMsg;
private: sensor_msgs::Image* leftImageMsg;
private: sensor_msgs::Image* rightImageMsg;
- private: sensor_msgs::CamInfo* leftCamInfoMsg;
- private: sensor_msgs::CamInfo* rightCamInfoMsg;
+ private: sensor_msgs::CameraInfo* leftCameraInfoMsg;
+ private: sensor_msgs::CameraInfo* rightCameraInfoMsg;
private: sensor_msgs::StereoInfo* stereoInfoMsg;
/// \brief A mutex to lock access to fields that are used in message callbacks
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_prosilica.cpp
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_prosilica.cpp 2009-07-31 22:10:05 UTC (rev 20323)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_prosilica.cpp 2009-07-31 22:17:14 UTC (rev 20324)
@@ -45,7 +45,7 @@
#include <sensor_msgs/Image.h>
-#include <sensor_msgs/CamInfo.h>
+#include <sensor_msgs/CameraInfo.h>
#include <sensor_msgs/FillImage.h>
#include <diagnostic_updater/diagnostic_updater.h>
@@ -163,19 +163,19 @@
ROS_DEBUG("prosilica image topic name %s", this->imageTopicName.c_str());
this->image_pub_ = this->rosnode_->advertise<sensor_msgs::Image>(this->imageTopicName,1);
this->image_rect_pub_ = this->rosnode_->advertise<sensor_msgs::Image>(this->imageRectTopicName,1);
- this->cam_info_pub_ = this->rosnode_->advertise<sensor_msgs::CamInfo>(this->camInfoTopicName,1);
+ this->cam_info_pub_ = this->rosnode_->advertise<sensor_msgs::CameraInfo>(this->camInfoTopicName,1);
this->cam_info_ser_ = this->rosnode_->advertiseService(this->camInfoServiceName,&RosProsilica::camInfoService, this);
this->poll_ser_ = this->rosnode_->advertiseService(this->pollServiceName,&RosProsilica::triggeredGrab, this);
}
////////////////////////////////////////////////////////////////////////////////
// service call to return camera info
-bool RosProsilica::camInfoService(prosilica_cam::CamInfo::Request &req,
- prosilica_cam::CamInfo::Response &res)
+bool RosProsilica::camInfoService(prosilica_cam::CameraInfo::Request &req,
+ prosilica_cam::CameraInfo::Response &res)
{
// should return the cam info for the entire camera frame
this->camInfoMsg = &res.cam_info;
- // fill CamInfo
+ // fill CameraInfo
this->camInfoMsg->header.frame_id = this->frameName;
this->camInfoMsg->header.stamp = ros::Time((unsigned long)floor(Simulator::Instance()->GetSimTime()));
this->camInfoMsg->height = this->myParent->GetImageHeight();
@@ -247,51 +247,51 @@
{
this->lock.lock();
- // fill CamInfo
- this->roiCamInfoMsg = &res.cam_info;
- this->roiCamInfoMsg->header.frame_id = this->frameName;
- this->roiCamInfoMsg->header.stamp = ros::Time((unsigned long)floor(Simulator::Instance()->GetSimTime()));
- this->roiCamInfoMsg->width = req.width; //this->myParent->GetImageWidth() ;
- this->roiCamInfoMsg->height = req.height; //this->myParent->GetImageHeight();
+ // fill CameraInfo
+ this->roiCameraInfoMsg = &res.cam_info;
+ this->roiCameraInfoMsg->header.frame_id = this->frameName;
+ this->roiCameraInfoMsg->header.stamp = ros::Time((unsigned long)floor(Simulator::Instance()->GetSimTime()));
+ this->roiCameraInfoMsg->width = req.width; //this->myParent->GetImageWidth() ;
+ this->roiCameraInfoMsg->height = req.height; //this->myParent->GetImageHeight();
// distortion
- this->roiCamInfoMsg->D[0] = 0.0;
- this->roiCamInfoMsg->D[1] = 0.0;
- this->roiCamInfoMsg->D[2] = 0.0;
- this->roiCamInfoMsg->D[3] = 0.0;
- this->roiCamInfoMsg->D[4] = 0.0;
+ this->roiCameraInfoMsg->D[0] = 0.0;
+ this->roiCameraInfoMsg->D[1] = 0.0;
+ this->roiCameraInfoMsg->D[2] = 0.0;
+ this->roiCameraInfoMsg->D[3] = 0.0;
+ this->roiCameraInfoMsg->D[4] = 0.0;
// original camera matrix
- this->roiCamInfoMsg->K[0] = this->focal_length;
- this->roiCamInfoMsg->K[1] = 0.0;
- this->roiCamInfoMsg->K[2] = this->Cx - req.region_x;
- this->roiCamInfoMsg->K[3] = 0.0;
- this->roiCamInfoMsg->K[4] = this->focal_length;
- this->roiCamInfoMsg->K[5] = this->Cy - req.region_y;
- this->roiCamInfoMsg->K[6] = 0.0;
- this->roiCamInfoMsg->K[7] = 0.0;
- this->roiCamInfoMsg->K[8] = 1.0;
+ this->roiCameraInfoMsg->K[0] = this->focal_length;
+ this->roiCameraInfoMsg->K[1] = 0.0;
+ this->roiCameraInfoMsg->K[2] = this->Cx - req.region_x;
+ this->roiCameraInfoMsg->K[3] = 0.0;
+ this->roiCameraInfoMsg->K[4] = this->focal_length;
+ this->roiCameraInfoMsg->K[5] = this->Cy - req.region_y;
+ this->roiCameraInfoMsg->K[6] = 0.0;
+ this->roiCameraInfoMsg->K[7] = 0.0;
+ this->roiCameraInfoMsg->K[8] = 1.0;
// rectification
- this->roiCamInfoMsg->R[0] = 1.0;
- this->roiCamInfoMsg->R[1] = 0.0;
- this->roiCamInfoMsg->R[2] = 0.0;
- this->roiCamInfoMsg->R[3] = 0.0;
- this->roiCamInfoMsg->R[4] = 1.0;
- this->roiCamInfoMsg->R[5] = 0.0;
- this->roiCamInfoMsg->R[6] = 0.0;
- this->roiCamInfoMsg->R[7] = 0.0;
- this->roiCamInfoMsg->R[8] = 1.0;
+ this->roiCameraInfoMsg->R[0] = 1.0;
+ this->roiCameraInfoMsg->R[1] = 0.0;
+ this->roiCameraInfoMsg->R[2] = 0.0;
+ this->roiCameraInfoMsg->R[3] = 0.0;
+ this->roiCameraInfoMsg->R[4] = 1.0;
+ this->roiCameraInfoMsg->R[5] = 0.0;
+ this->roiCameraInfoMsg->R[6] = 0.0;
+ this->roiCameraInfoMsg->R[7] = 0.0;
+ this->roiCameraInfoMsg->R[8] = 1.0;
// camera projection matrix (same as camera matrix due to lack of distortion/rectification) (is this generated?)
- this->roiCamInfoMsg->P[0] = this->focal_length;
- this->roiCamInfoMsg->P[1] = 0.0;
- this->roiCamInfoMsg->P[2] = this->Cx - req.region_x;
- this->roiCamInfoMsg->P[3] = 0.0;
- this->roiCamInfoMsg->P[4] = 0.0;
- this->roiCamInfoMsg->P[5] = this->focal_length;
- this->roiCamInfoMsg->P[6] = this->Cy - req.region_y;
- this->roiCamInfoMsg->P[7] = 0.0;
- this->roiCamInfoMsg->P[8] = 0.0;
- this->roiCamInfoMsg->P[9] = 0.0;
- this->roiCamInfoMsg->P[10] = 1.0;
- this->roiCamInfoMsg->P[11] = 0.0;
+ this->roiCameraInfoMsg->P[0] = this->focal_length;
+ this->roiCameraInfoMsg->P[1] = 0.0;
+ this->roiCameraInfoMsg->P[2] = this->Cx - req.region_x;
+ this->roiCameraInfoMsg->P[3] = 0.0;
+ this->roiCameraInfoMsg->P[4] = 0.0;
+ this->roiCameraInfoMsg->P[5] = this->focal_length;
+ this->roiCameraInfoMsg->P[6] = this->Cy - req.region_y;
+ this->roiCameraInfoMsg->P[7] = 0.0;
+ this->roiCameraInfoMsg->P[8] = 0.0;
+ this->roiCameraInfoMsg->P[9] = 0.0;
+ this->roiCameraInfoMsg->P[10] = 1.0;
+ this->roiCameraInfoMsg->P[11] = 0.0;
// copy data into image
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_stereo_camera.cpp
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_stereo_camera.cpp 2009-07-31 22:10:05 UTC (rev 20323)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_stereo_camera.cpp 2009-07-31 22:17:14 UTC (rev 20324)
@@ -82,8 +82,8 @@
// RawStereo.msg
this->leftImageMsg = &(this->rawStereoMsg.left_image);
this->rightImageMsg = &(this->rawStereoMsg.right_image);
- this->leftCamInfoMsg = &(this->rawStereoMsg.left_info);
- this->rightCamInfoMsg = &(this->rawStereoMsg.right_info);
+ this->leftCameraInfoMsg = &(this->rawStereoMsg.left_info);
+ this->rightCameraInfoMsg = &(this->rawStereoMsg.right_info);
this->stereoInfoMsg = &(this->rawStereoMsg.stereo_info);
ROS_DEBUG("stereo: done with constuctor");
@@ -323,93 +323,93 @@
this->stereoInfoMsg->RP[14] = -1.0/this->baseline;
this->stereoInfoMsg->RP[15] = (this->Cx-this->CxPrime)/this->baseline;
- // fill CamInfo left_info
- this->leftCamInfoMsg->header = this->rawStereoMsg.header;
- this->leftCamInfoMsg->height = this->leftCamera->GetImageHeight();
- this->leftCamInfoMsg->width = this->leftCamera->GetImageWidth() ;
+ // fill CameraInfo left_info
+ this->leftCameraInfoMsg->header = this->rawStereoMsg.header;
+ this->leftCameraInfoMsg->height = this->leftCamera->GetImageHeight();
+ this->leftCameraInfoMsg->width = this->leftCamera->GetImageWidth() ;
// distortion
- this->leftCamInfoMsg->D[0] = 0.0;
- this->leftCamInfoMsg->D[1] = 0.0;
- this->leftCamInfoMsg->D[2] = 0.0;
- this->leftCamInfoMsg->D[3] = 0.0;
- this->leftCamInfoMsg->D[4] = 0.0;
+ this->leftCameraInfoMsg->D[0] = 0.0;
+ this->leftCameraInfoMsg->D[1] = 0.0;
+ this->leftCameraInfoMsg->D[2] = 0.0;
+ this->leftCameraInfoMsg->D[3] = 0.0;
+ this->leftCameraInfoMsg->D[4] = 0.0;
// original camera matrix
- this->leftCamInfoMsg->K[0] = this->focal_length;
- this->leftCamInfoMsg->K[1] = 0.0;
- this->leftCamInfoMsg->K[2] = this->Cx;
- this->leftCamInfoMsg->K[3] = 0.0;
- this->leftCamInfoMsg->K[4] = this->focal_length;
- this->leftCamInfoMsg->K[5] = this->Cy;
- this->leftCamInfoMsg->K[6] = 0.0;
- this->leftCamInfoMsg->K[7] = 0.0;
- this->leftCamInfoMsg->K[8] = 1.0;
+ this->leftCameraInfoMsg->K[0] = this->focal_length;
+ this->leftCameraInfoMsg->K[1] = 0.0;
+ this->leftCameraInfoMsg->K[2] = this->Cx;
+ this->leftCameraInfoMsg->K[3] = 0.0;
+ this->leftCameraInfoMsg->K[4] = this->focal_length;
+ this->leftCameraInfoMsg->K[5] = this->Cy;
+ this->leftCameraInfoMsg->K[6] = 0.0;
+ this->leftCameraInfoMsg->K[7] = 0.0;
+ this->leftCameraInfoMsg->K[8] = 1.0;
// rectification
- this->leftCamInfoMsg->R[0] = 1.0;
- this->leftCamInfoMsg->R[1] = 0.0;
- this->leftCamInfoMsg->R[2] = 0.0;
- this->leftCamInfoMsg->R[3] = 0.0;
- this->leftCamInfoMsg->R[4] = 1.0;
- this->leftCamInfoMsg->R[5] = 0.0;
- this->leftCamInfoMsg->R[6] = 0.0;
- this->leftCamInfoMsg->R[7] = 0.0;
- this->leftCamInfoMsg->R[8] = 1.0;
+ this->leftCameraInfoMsg->R[0] = 1.0;
+ this->leftCameraInfoMsg->R[1] = 0.0;
+ this->leftCameraInfoMsg->R[2] = 0.0;
+ this->leftCameraInfoMsg->R[3] = 0.0;
+ this->leftCameraInfoMsg->R[4] = 1.0;
+ this->leftCameraInfoMsg->R[5] = 0.0;
+ this->leftCameraInfoMsg->R[6] = 0.0;
+ this->leftCameraInfoMsg->R[7] = 0.0;
+ this->leftCameraInfoMsg->R[8] = 1.0;
// camera projection matrix (same as camera matrix due to lack of distortion/rectification) (is this generated?)
- this->leftCamInfoMsg->P[0] = this->focal_length;
- this->leftCamInfoMsg->P[1] = 0.0;
- this->leftCamInfoMsg->P[2] = this->Cx;
- this->leftCamInfoMsg->P[3] = 0.0;
- this->leftCamInfoMsg->P[4] = 0.0;
- this->leftCamInfoMsg->P[5] = this->focal_length;
- this->leftCamInfoMsg->P[6] = this->Cy;
- this->leftCamInfoMsg->P[7] = 0.0;
- this->leftCamInfoMsg->P[8] = 0.0;
- this->leftCamInfoMsg->P[9] = 0.0;
- this->leftCamInfoMsg->P[10] = 1.0;
- this->leftCamInfoMsg->P[11] = 0.0;
+ this->leftCameraInfoMsg->P[0] = this->focal_length;
+ this->leftCameraInfoMsg->P[1] = 0.0;
+ this->leftCameraInfoMsg->P[2] = this->Cx;
+ this->leftCameraInfoMsg->P[3] = 0.0;
+ this->leftCameraInfoMsg->P[4] = 0.0;
+ this->leftCameraInfoMsg->P[5] = this->focal_length;
+ this->leftCameraInfoMsg->P[6] = this->Cy;
+ this->leftCameraInfoMsg->P[7] = 0.0;
+ this->leftCameraInfoMsg->P[8] = 0.0;
+ this->leftCameraInfoMsg->P[9] = 0.0;
+ this->leftCameraInfoMsg->P[10] = 1.0;
+ this->leftCameraInfoMsg->P[11] = 0.0;
- // fill CamInfo right_info
- this->rightCamInfoMsg->header = this->rawStereoMsg.header;
- this->rightCamInfoMsg->height = this->rightCamera->GetImageHeight();
- this->rightCamInfoMsg->width = this->rightCamera->GetImageWidth() ;
+ // fill CameraInfo right_info
+ this->rightCameraInfoMsg->header = this->rawStereoMsg.header;
+ this->rightCameraInfoMsg->height = this->rightCamera->GetImageHeight();
+ this->rightCameraInfoMsg->width = this->rightCamera->GetImageWidth() ;
// distortion
- this->rightCamInfoMsg->D[0] = 0.0;
- this->rightCamInfoMsg->D[1] = 0.0;
- this->rightCamInfoMsg->D[2] = 0.0;
- this->rightCamInfoMsg->D[3] = 0.0;
- this->rightCamInfoMsg->D[4] = 0.0;
+ this->rightCameraInfoMsg->D[0] = 0.0;
+ this->rightCameraInfoMsg->D[1] = 0.0;
+ this->rightCameraInfoMsg->D[2] = 0.0;
+ this->rightCameraInfoMsg->D[3] = 0.0;
+ this->rightCameraInfoMsg->D[4] = 0.0;
// original camera matrix
- this->rightCamInfoMsg->K[0] = this->focal_length;
- this->rightCamInfoMsg->K[1] = 0.0;
- this->rightCamInfoMsg->K[2] = this->Cx;
- this->rightCamInfoMsg->K[3] = 0.0;
- this->rightCamInfoMsg->K[4] = this->focal_length;
- this->rightCamInfoMsg->K[5] = this->Cy;
- this->rightCamInfoMsg->K[6] = 0.0;
- this->rightCamInfoMsg->K[7] = 0.0;
- this->rightCamInfoMsg->K[8] = 1.0;
+ this->rightCameraInfoMsg->K[0] = this->focal_length;
+ this->rightCameraInfoMsg->K[1] = 0.0;
+ this->rightCameraInfoMsg->K[2] = this->Cx;
+ this->rightCameraInfoMsg->K[3] = 0.0;
+ this->rightCameraInfoMsg->K[4] = this->focal_length;
+ this->rightCameraInfoMsg->K[5] = this->Cy;
+ this->rightCameraInfoMsg->K[6] = 0.0;
+ this->rightCameraInfoMsg->K[7] = 0.0;
+ this->rightCameraInfoMsg->K[8] = 1.0;
// rectification
- this->rightCamInfoMsg->R[0] = 1.0;
- this->rightCamInfoMsg->R[1] = 0.0;
- this->rightCamInfoMsg->R[2] = 0.0;
- this->rightCamInfoMsg->R[3] = 0.0;
- this->rightCamInfoMsg->R[4] = 1.0;
- this->rightCamInfoMsg->R[5] = 0.0;
- this->rightCamInfoMsg->R[6] = 0.0;
- this->rightCamInfoMsg->R[7] = 0.0;
- this->rightCamInfoMsg->R[8] = 1.0;
+ this->rightCameraInfoMsg->R[0] = 1.0;
+ this->rightCameraInfoMsg->R[1] = 0.0;
+ this->rightCameraInfoMsg->R[2] = 0.0;
+ this->rightCameraInfoMsg->R[3] = 0.0;
+ this->rightCameraInfoMsg->R[4] = 1.0;
+ this->rightCameraInfoMsg->R[5] = 0.0;
+ this->rightCameraInfoMsg->R[6] = 0.0;
+ this->rightCameraInfoMsg->R[7] = 0.0;
+ this->rightCameraInfoMsg->R[8] = 1.0;
// camera projection matrix (same as camera matrix due to lack of distortion/rectification) (is this generated?)
- this->rightCamInfoMsg->P[0] = this->focal_length;
- this->rightCamInfoMsg->P[1] = 0.0;
- this->rightCamInfoMsg->P[2] = this->Cx;
- this->rightCamInfoMsg->P[3] = -this->focal_length*this->baseline;
- this->rightCamInfoMsg->P[4] = 0.0;
- this->rightCamInfoMsg->P[5] = this->focal_length;
- this->rightCamInfoMsg->P[6] = this->Cy;
- this->rightCamInfoMsg->P[7] = ...
[truncated message content] |
|
From: <is...@us...> - 2009-08-02 20:45:07
|
Revision: 20443
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=20443&view=rev
Author: isucan
Date: 2009-08-02 20:44:59 +0000 (Sun, 02 Aug 2009)
Log Message:
-----------
clean some function names, began work on concave shapes
Modified Paths:
--------------
pkg/trunk/motion_planning/planning_environment/src/util/construct_object.cpp
pkg/trunk/motion_planning/planning_models/src/kinematic.cpp
pkg/trunk/util/geometric_shapes/include/geometric_shapes/bodies.h
pkg/trunk/util/geometric_shapes/include/geometric_shapes/shapes.h
pkg/trunk/util/geometric_shapes/src/bodies.cpp
pkg/trunk/util/geometric_shapes/src/load_mesh.cpp
pkg/trunk/util/geometric_shapes/src/shapes.cpp
pkg/trunk/world_models/collision_space/src/environmentODE.cpp
pkg/trunk/world_models/collision_space/src/environment_objects.cpp
Modified: pkg/trunk/motion_planning/planning_environment/src/util/construct_object.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_environment/src/util/construct_object.cpp 2009-08-02 19:42:28 UTC (rev 20442)
+++ pkg/trunk/motion_planning/planning_environment/src/util/construct_object.cpp 2009-08-02 20:44:59 UTC (rev 20443)
@@ -84,7 +84,7 @@
vertices[i].setValue(obj.vertices[i].x, obj.vertices[i].y, obj.vertices[i].z);
for (unsigned int i = 0 ; i < obj.triangles.size() ; ++i)
triangles[i] = obj.triangles[i];
- shape = shapes::create_mesh_from_vertices(vertices, triangles);
+ shape = shapes::createMeshFromVertices(vertices, triangles);
}
}
}
Modified: pkg/trunk/motion_planning/planning_models/src/kinematic.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_models/src/kinematic.cpp 2009-08-02 19:42:28 UTC (rev 20442)
+++ pkg/trunk/motion_planning/planning_models/src/kinematic.cpp 2009-08-02 20:44:59 UTC (rev 20443)
@@ -265,7 +265,7 @@
if (filename.rfind(".stl") == std::string::npos)
filename += ".stl";
// std::cout << "Loading '" << filename << "'" << std::endl;
- shapes::Mesh *mesh = shapes::create_mesh_from_binary_stl(filename.c_str());
+ shapes::Mesh *mesh = shapes::createMeshFromBinaryStl(filename.c_str());
shape = mesh;
}
break;
@@ -852,12 +852,12 @@
dest->constGeomTrans = src->constGeomTrans;
dest->globalTransFwd = src->globalTransFwd;
dest->globalTrans = src->globalTrans;
- dest->shape = shapes::clone_shape(src->shape);
+ dest->shape = shapes::cloneShape(src->shape);
for (unsigned int i = 0 ; i < src->attachedBodies.size() ; ++i)
{
AttachedBody *ab = new AttachedBody(dest);
ab->attachTrans = src->attachedBodies[i]->attachTrans;
- ab->shape = shapes::clone_shape(src->attachedBodies[i]->shape);
+ ab->shape = shapes::cloneShape(src->attachedBodies[i]->shape);
ab->globalTrans = src->attachedBodies[i]->globalTrans;
dest->attachedBodies.push_back(ab);
}
Modified: pkg/trunk/util/geometric_shapes/include/geometric_shapes/bodies.h
===================================================================
--- pkg/trunk/util/geometric_shapes/include/geometric_shapes/bodies.h 2009-08-02 19:42:28 UTC (rev 20442)
+++ pkg/trunk/util/geometric_shapes/include/geometric_shapes/bodies.h 2009-08-02 20:44:59 UTC (rev 20443)
@@ -39,6 +39,8 @@
#include "geometric_shapes/shapes.h"
#include <LinearMath/btTransform.h>
+#include <BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h>
+#include <BulletCollision/CollisionShapes/btTriangleMesh.h>
#include <vector>
/**
@@ -295,7 +297,61 @@
double m_height2;
double m_radiusB;
};
+
+ class Mesh : public Body
+ {
+ Mesh(void) : Body()
+ {
+ m_type = shapes::MESH;
+ m_btMeshShape = NULL;
+ m_btMesh = NULL;
+ m_center.setValue(0, 0, 0);
+ m_aabbMin.setValue(0, 0, 0);
+ m_aabbMax.setValue(0, 0, 0);
+ }
+
+ Mesh(const shapes::Shape *shape) : Body()
+ {
+ m_type = shapes::MESH;
+ m_btMeshShape = NULL;
+ m_btMesh = NULL;
+ m_center.setValue(0, 0, 0);
+ m_aabbMin.setValue(0, 0, 0);
+ m_aabbMax.setValue(0, 0, 0);
+ setDimensions(shape);
+ }
+
+ virtual ~Mesh(void)
+ {
+ if (m_btMeshShape)
+ delete m_btMeshShape;
+ if (m_btMesh)
+ delete m_btMesh;
+ }
+
+ /** \brief The mesh is considered to be concave, so this function is implemented with raycasting. This is a bit slow. */
+ virtual bool containsPoint(const btVector3 &p) const;
+
+ /** \brief This function is approximate. It returns the volume of the AABB enclosing the shape */
+ virtual double computeVolume(void) const;
+ virtual void computeBoundingSphere(BoundingSphere &sphere) const;
+ virtual bool intersectsRay(const btVector3& origin, const btVector3 &dir, std::vector<btVector3> *intersections = NULL, unsigned int count = 0);
+
+ protected:
+
+ virtual void useDimensions(const shapes::Shape *shape);
+ virtual void updateInternalData(void);
+
+ btBvhTriangleMeshShape *m_btMeshShape;
+ btTriangleMesh *m_btMesh;
+ btTransform m_iPose;
+ btVector3 m_center;
+ btVector3 m_aabbMin;
+ btVector3 m_aabbMax;
+
+ };
+
/** \brief Definition of a convex mesh. Convex hull is computed for a given shape::Mesh */
class ConvexMesh : public Body
{
@@ -320,8 +376,10 @@
{
}
+
virtual bool containsPoint(const btVector3 &p) const;
virtual double computeVolume(void) const;
+
virtual void computeBoundingSphere(BoundingSphere &sphere) const;
virtual bool intersectsRay(const btVector3& origin, const btVector3 &dir, std::vector<btVector3> *intersections = NULL, unsigned int count = 0);
Modified: pkg/trunk/util/geometric_shapes/include/geometric_shapes/shapes.h
===================================================================
--- pkg/trunk/util/geometric_shapes/include/geometric_shapes/shapes.h 2009-08-02 19:42:28 UTC (rev 20442)
+++ pkg/trunk/util/geometric_shapes/include/geometric_shapes/shapes.h 2009-08-02 20:44:59 UTC (rev 20443)
@@ -225,23 +225,23 @@
constructed using index values from the triangles
vector. Triangle k has vertices at index values triangles[3k],
triangles[3k+1], triangles[3k+2] */
- Mesh* create_mesh_from_vertices(const std::vector<btVector3> &vertices, const std::vector<unsigned int> &triangles);
+ Mesh* createMeshFromVertices(const std::vector<btVector3> &vertices, const std::vector<unsigned int> &triangles);
/** \brief Load a mesh from a set of vertices. Every 3 vertices
are considered a triangle. Repeating vertices are identified
and the set of triangle indices is constructed. The normal at
each triangle is also computed */
- Mesh* create_mesh_from_vertices(const std::vector<btVector3> &source);
+ Mesh* createMeshFromVertices(const std::vector<btVector3> &source);
/** \brief Load a mesh from a binary STL file. Normals are
recomputed and repeating vertices are identified. */
- Mesh* create_mesh_from_binary_stl(const char *filename);
+ Mesh* createMeshFromBinaryStl(const char *filename);
/** \brief Create a copy of a shape */
- Shape* clone_shape(const Shape *shape);
+ Shape* cloneShape(const Shape *shape);
/** \brief Create a copy of a static shape */
- StaticShape* clone_shape(const StaticShape *shape);
+ StaticShape* cloneShape(const StaticShape *shape);
}
Modified: pkg/trunk/util/geometric_shapes/src/bodies.cpp
===================================================================
--- pkg/trunk/util/geometric_shapes/src/bodies.cpp 2009-08-02 19:42:28 UTC (rev 20442)
+++ pkg/trunk/util/geometric_shapes/src/bodies.cpp 2009-08-02 20:44:59 UTC (rev 20443)
@@ -36,6 +36,7 @@
#include "geometric_shapes/bodies.h"
#include <LinearMath/btConvexHull.h>
+#include <BulletCollision/CollisionShapes/btTriangleShape.h>
#include <algorithm>
#include <iostream>
#include <cmath>
@@ -469,6 +470,133 @@
return true;
}
+bool bodies::Mesh::containsPoint(const btVector3 &p) const
+{
+ return false;
+}
+
+namespace bodies
+{
+ namespace detail
+ {
+ class myTriangleCallback : public btTriangleCallback
+ {
+ public:
+
+ myTriangleCallback(bool keep_triangles) : keep_triangles_(keep_triangles)
+ {
+ found_intersections = false;
+ }
+
+ virtual void processTriangle(btVector3 *triangle, int partId, int triangleIndex)
+ {
+ found_intersections = true;
+ if (keep_triangles_)
+ triangles.push_back(btTriangleShape(triangle[0], triangle[1], triangle[2]));
+ }
+
+ bool found_intersections;
+ std::vector<btTriangleShape> triangles;
+
+ private:
+
+ bool keep_triangles_;
+ };
+ }
+}
+
+bool bodies::Mesh::intersectsRay(const btVector3& origin, const btVector3& dir, std::vector<btVector3> *intersections, unsigned int count)
+{
+ if (m_btMeshShape)
+ {
+ // transform the ray into the coordinate frame of the mesh
+ btVector3 orig(m_iPose * origin);
+ btVector3 dr(m_iPose.getBasis() * dir);
+ /// \todo should do intersection with AABB first; the target for performRayCast is the intersection if the box
+ detail::myTriangleCallback cb(intersections != NULL);
+ m_btMeshShape->performRaycast(&cb, orig, dr);
+ if (cb.found_intersections)
+ {
+ if (intersections)
+ {
+ /// \todo find the intersections with list of triangles
+ }
+ return true;
+ }
+ else
+ return false;
+ }
+ else
+ return false;
+}
+
+void bodies::Mesh::useDimensions(const shapes::Shape *shape)
+{
+ const shapes::Mesh *mesh = static_cast<const shapes::Mesh*>(shape);
+ if (m_btMeshShape)
+ delete m_btMeshShape;
+ if (m_btMesh)
+ delete m_btMesh;
+
+ m_btMesh = new btTriangleMesh();
+ const unsigned int nt = mesh->triangleCount / 3;
+ for (unsigned int i = 0 ; i < nt ; ++i)
+ {
+ const unsigned int i3 = i *3;
+ const unsigned int v1 = 3 * mesh->triangles[i3];
+ const unsigned int v2 = 3 * mesh->triangles[i3 + 1];
+ const unsigned int v3 = 3 * mesh->triangles[i3 + 2];
+ m_btMesh->addTriangle(btVector3(mesh->vertices[v1], mesh->vertices[v1 + 1], mesh->vertices[v1 + 2]),
+ btVector3(mesh->vertices[v2], mesh->vertices[v2 + 1], mesh->vertices[v2 + 2]),
+ btVector3(mesh->vertices[v3], mesh->vertices[v3 + 1], mesh->vertices[v3 + 2]), true);
+ }
+
+ m_btMeshShape = new btBvhTriangleMeshShape(m_btMesh, true);
+}
+
+void bodies::Mesh::updateInternalData(void)
+{
+ if (m_btMeshShape)
+ {
+ m_btMeshShape->setLocalScaling(btVector3(m_scale, m_scale, m_scale));
+ m_btMeshShape->setMargin(m_padding);
+ }
+ m_iPose = m_pose.inverse();
+ m_center = m_pose.getOrigin();
+
+ btTransform id;
+ id.setIdentity();
+ m_btMeshShape->getAabb(id, m_aabbMin, m_aabbMax);
+ /// \todo check if the AABB computation includes padding & scaling; if not, include it
+}
+
+double bodies::Mesh::computeVolume(void) const
+{
+ if (m_btMeshShape)
+ {
+ // approximation; volume of AABB at identity transform
+ btVector3 d = m_aabbMax - m_aabbMin;
+ return d.x() * d.y() * d.z();
+ }
+ else
+ return 0.0;
+}
+
+void bodies::Mesh::computeBoundingSphere(BoundingSphere &sphere) const
+{
+ if (m_btMeshShape)
+ {
+ sphere.center = m_center + (m_aabbMax + m_aabbMin) / 2.0;
+ btVector3 d = (m_aabbMax - m_aabbMin) / 2.0;
+ sphere.radius = d.length();
+ }
+ else
+ {
+ sphere.center = m_center;
+ sphere.radius = 0.0;
+ }
+}
+
bool bodies::ConvexMesh::containsPoint(const btVector3 &p) const
{
// if (m_boundingBox.containsPoint(p))
Modified: pkg/trunk/util/geometric_shapes/src/load_mesh.cpp
===================================================================
--- pkg/trunk/util/geometric_shapes/src/load_mesh.cpp 2009-08-02 19:42:28 UTC (rev 20442)
+++ pkg/trunk/util/geometric_shapes/src/load_mesh.cpp 2009-08-02 20:44:59 UTC (rev 20443)
@@ -80,7 +80,7 @@
};
}
- shapes::Mesh* create_mesh_from_vertices(const std::vector<btVector3> &vertices, const std::vector<unsigned int> &triangles)
+ shapes::Mesh* createMeshFromVertices(const std::vector<btVector3> &vertices, const std::vector<unsigned int> &triangles)
{
unsigned int nt = triangles.size() / 3;
shapes::Mesh *mesh = new shapes::Mesh(vertices.size(), nt);
@@ -107,7 +107,7 @@
return mesh;
}
- shapes::Mesh* create_mesh_from_vertices(const std::vector<btVector3> &source)
+ shapes::Mesh* createMeshFromVertices(const std::vector<btVector3> &source)
{
if (source.size() < 3)
return NULL;
@@ -188,7 +188,7 @@
return mesh;
}
- shapes::Mesh* create_mesh_from_binary_stl(const char *filename)
+ shapes::Mesh* createMeshFromBinaryStl(const char *filename)
{
FILE* input = fopen(filename, "r");
@@ -258,7 +258,7 @@
delete[] buffer;
- return create_mesh_from_vertices(vertices);
+ return createMeshFromVertices(vertices);
}
}
Modified: pkg/trunk/util/geometric_shapes/src/shapes.cpp
===================================================================
--- pkg/trunk/util/geometric_shapes/src/shapes.cpp 2009-08-02 19:42:28 UTC (rev 20442)
+++ pkg/trunk/util/geometric_shapes/src/shapes.cpp 2009-08-02 20:44:59 UTC (rev 20443)
@@ -36,7 +36,7 @@
#include "geometric_shapes/shapes.h"
-shapes::Shape* shapes::clone_shape(const shapes::Shape *shape)
+shapes::Shape* shapes::cloneShape(const shapes::Shape *shape)
{
shapes::Shape *result = NULL;
switch (shape->type)
@@ -72,7 +72,7 @@
return result;
}
-shapes::StaticShape* shapes::clone_shape(const shapes::StaticShape *shape)
+shapes::StaticShape* shapes::cloneShape(const shapes::StaticShape *shape)
{
shapes::StaticShape *result = NULL;
switch (shape->type)
Modified: pkg/trunk/world_models/collision_space/src/environmentODE.cpp
===================================================================
--- pkg/trunk/world_models/collision_space/src/environmentODE.cpp 2009-08-02 19:42:28 UTC (rev 20442)
+++ pkg/trunk/world_models/collision_space/src/environmentODE.cpp 2009-08-02 20:44:59 UTC (rev 20443)
@@ -966,13 +966,13 @@
int idx = shapePtrs[dGeomGetData(it->second->geoms[i])];
if (idx < 0) // static geom
{
- shapes::StaticShape *newShape = shapes::clone_shape(ns.staticShape[-idx - 1]);
+ shapes::StaticShape *newShape = shapes::cloneShape(ns.staticShape[-idx - 1]);
dGeomSetData(newGeom, reinterpret_cast<void*>(newShape));
env->m_objects->addObject(it->first, newShape);
}
else // movable geom
{
- shapes::Shape *newShape = shapes::clone_shape(ns.shape[idx]);
+ shapes::Shape *newShape = shapes::cloneShape(ns.shape[idx]);
dGeomSetData(newGeom, reinterpret_cast<void*>(newShape));
env->m_objects->addObject(it->first, newShape, ns.shapePose[idx]);
}
@@ -987,13 +987,13 @@
int idx = shapePtrs[dGeomGetData(geoms[i])];
if (idx < 0) // static geom
{
- shapes::StaticShape *newShape = shapes::clone_shape(ns.staticShape[-idx - 1]);
+ shapes::StaticShape *newShape = shapes::cloneShape(ns.staticShape[-idx - 1]);
dGeomSetData(newGeom, reinterpret_cast<void*>(newShape));
env->m_objects->addObject(it->first, newShape);
}
else // movable geom
{
- shapes::Shape *newShape = shapes::clone_shape(ns.shape[idx]);
+ shapes::Shape *newShape = shapes::cloneShape(ns.shape[idx]);
dGeomSetData(newGeom, reinterpret_cast<void*>(newShape));
env->m_objects->addObject(it->first, newShape, ns.shapePose[idx]);
}
Modified: pkg/trunk/world_models/collision_space/src/environment_objects.cpp
===================================================================
--- pkg/trunk/world_models/collision_space/src/environment_objects.cpp 2009-08-02 19:42:28 UTC (rev 20442)
+++ pkg/trunk/world_models/collision_space/src/environment_objects.cpp 2009-08-02 20:44:59 UTC (rev 20443)
@@ -132,10 +132,10 @@
NamespaceObjects &ns = c->m_objects[it->first];
unsigned int n = it->second.staticShape.size();
for (unsigned int i = 0 ; i < n ; ++i)
- ns.staticShape.push_back(shapes::clone_shape(it->second.staticShape[i]));
+ ns.staticShape.push_back(shapes::cloneShape(it->second.staticShape[i]));
n = it->second.shape.size();
for (unsigned int i = 0 ; i < n ; ++i)
- ns.shape.push_back(shapes::clone_shape(it->second.shape[i]));
+ ns.shape.push_back(shapes::cloneShape(it->second.shape[i]));
ns.shapePose = it->second.shapePose;
}
return c;
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <hsu...@us...> - 2009-08-02 21:02:53
|
Revision: 20444
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=20444&view=rev
Author: hsujohnhsu
Date: 2009-08-02 21:02:43 +0000 (Sun, 02 Aug 2009)
Log Message:
-----------
split up gazebo_tools from gazebo_plugin. update corresponding launch scripts.
Modified Paths:
--------------
pkg/trunk/demos/arm_gazebo/l_arm.launch
pkg/trunk/demos/arm_gazebo/l_gripper.launch
pkg/trunk/demos/arm_gazebo/r_arm.launch
pkg/trunk/demos/arm_gazebo/r_arm_grasping_demo.launch
pkg/trunk/demos/arm_gazebo/r_gripper.launch
pkg/trunk/demos/arm_gazebo/scripts/test_launch_order.tcsh
pkg/trunk/demos/arm_gazebo/tests/r_arm_constraint.launch
pkg/trunk/demos/arm_gazebo/tests/r_arm_spacenav.launch
pkg/trunk/demos/door_demos_gazebo/door_defs/Makefile
pkg/trunk/demos/door_demos_gazebo/launch/door_only_test.launch
pkg/trunk/demos/door_demos_gazebo/launch/pr2_and_door.launch
pkg/trunk/demos/erratic_gazebo/erratic_demo.launch
pkg/trunk/demos/examples_gazebo/coffee_cup.launch
pkg/trunk/demos/examples_gazebo/cups.launch
pkg/trunk/demos/examples_gazebo/cups2.launch
pkg/trunk/demos/examples_gazebo/dual_link.launch
pkg/trunk/demos/examples_gazebo/dual_link_defs/Makefile
pkg/trunk/demos/examples_gazebo/multi_link.launch
pkg/trunk/demos/examples_gazebo/multi_link_defs/Makefile
pkg/trunk/demos/examples_gazebo/simple_box.launch
pkg/trunk/demos/examples_gazebo/simple_cylinder.launch
pkg/trunk/demos/examples_gazebo/simple_sphere.launch
pkg/trunk/demos/examples_gazebo/single_link.launch
pkg/trunk/demos/examples_gazebo/single_link_defs/Makefile
pkg/trunk/demos/examples_gazebo/table.launch
pkg/trunk/demos/examples_gazebo/table_defs/Makefile
pkg/trunk/demos/examples_gazebo/tables.launch
pkg/trunk/demos/plug_in_gazebo/launch/plug_outlet.launch
pkg/trunk/demos/plug_in_gazebo/launch/pr2_plug_outlet.launch
pkg/trunk/demos/plug_in_gazebo/plug_defs/Makefile
pkg/trunk/demos/pr2_gazebo/coffee_cup.launch
pkg/trunk/demos/pr2_gazebo/head_cart.launch
pkg/trunk/demos/pr2_gazebo/pr2.launch
pkg/trunk/demos/pr2_gazebo/prf.launch
pkg/trunk/demos/pr2_gazebo/prototype1.launch
pkg/trunk/demos/pr2_gazebo/test_grasping.launch
pkg/trunk/drivers/simulator/gazebo_plugin/CMakeLists.txt
pkg/trunk/drivers/simulator/gazebo_plugin/doc.dox
pkg/trunk/drivers/simulator/gazebo_plugin/manifest.xml
pkg/trunk/drivers/simulator/test_pr2_collision_gazebo/test_slide.launch
pkg/trunk/drivers/simulator/test_pr2_sensors_gazebo/test_camera.launch
pkg/trunk/drivers/simulator/test_pr2_sensors_gazebo/test_scan.launch
Added Paths:
-----------
pkg/trunk/sandbox/gazebo_tools/
pkg/trunk/sandbox/gazebo_tools/CMakeLists.txt
pkg/trunk/sandbox/gazebo_tools/Makefile
pkg/trunk/sandbox/gazebo_tools/include/
pkg/trunk/sandbox/gazebo_tools/include/gazebo_tools/
pkg/trunk/sandbox/gazebo_tools/include/gazebo_tools/urdf2gazebo.h
pkg/trunk/sandbox/gazebo_tools/manifest.xml
pkg/trunk/sandbox/gazebo_tools/src/
pkg/trunk/sandbox/gazebo_tools/src/remove_model.cpp
pkg/trunk/sandbox/gazebo_tools/src/urdf2factory.cpp
pkg/trunk/sandbox/gazebo_tools/src/urdf2file.cpp
pkg/trunk/sandbox/gazebo_tools/src/urdf2gazebo.cpp
pkg/trunk/sandbox/gazebo_tools/src/xml2factory.cpp
Removed Paths:
-------------
pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/urdf2gazebo.h
pkg/trunk/drivers/simulator/gazebo_plugin/src/remove_model.cpp
pkg/trunk/drivers/simulator/gazebo_plugin/src/urdf2factory.cpp
pkg/trunk/drivers/simulator/gazebo_plugin/src/urdf2file.cpp
pkg/trunk/drivers/simulator/gazebo_plugin/src/urdf2gazebo.cpp
pkg/trunk/drivers/simulator/gazebo_plugin/src/xml2factory.cpp
Modified: pkg/trunk/demos/arm_gazebo/l_arm.launch
===================================================================
--- pkg/trunk/demos/arm_gazebo/l_arm.launch 2009-08-02 20:44:59 UTC (rev 20443)
+++ pkg/trunk/demos/arm_gazebo/l_arm.launch 2009-08-02 21:02:43 UTC (rev 20444)
@@ -4,7 +4,7 @@
<param name="robot_description" command="$(find xacro)/xacro.py '$(find pr2_defs)/robots/l_arm.xacro.xml'" />
<!-- push robot_description to factory and spawn robot in gazebo -->
- <node pkg="gazebo_plugin" type="urdf2factory" args="robot_description" respawn="false" output="screen"/>
+ <node pkg="gazebo_tools" type="urdf2factory" args="robot_description" respawn="false" output="screen"/>
<!-- start arm controller -->
<!--
Modified: pkg/trunk/demos/arm_gazebo/l_gripper.launch
===================================================================
--- pkg/trunk/demos/arm_gazebo/l_gripper.launch 2009-08-02 20:44:59 UTC (rev 20443)
+++ pkg/trunk/demos/arm_gazebo/l_gripper.launch 2009-08-02 21:02:43 UTC (rev 20444)
@@ -4,7 +4,7 @@
<param name="robot_description" command="$(find xacro)/xacro.py '$(find pr2_defs)/robots/l_gripper.xacro.xml'" />
<!-- push robot_description to factory and spawn robot in gazebo -->
- <node pkg="gazebo_plugin" type="urdf2factory" args="robot_description" respawn="false" output="screen"/>
+ <node pkg="gazebo_tools" type="urdf2factory" args="robot_description" respawn="false" output="screen"/>
<!-- start arm controller -->
<!--
Modified: pkg/trunk/demos/arm_gazebo/r_arm.launch
===================================================================
--- pkg/trunk/demos/arm_gazebo/r_arm.launch 2009-08-02 20:44:59 UTC (rev 20443)
+++ pkg/trunk/demos/arm_gazebo/r_arm.launch 2009-08-02 21:02:43 UTC (rev 20444)
@@ -4,7 +4,7 @@
<param name="robot_description" command="$(find xacro)/xacro.py '$(find pr2_defs)/robots/r_arm.xacro.xml'" />
<!-- push robot_description to factory and spawn robot in gazebo -->
- <node pkg="gazebo_plugin" type="urdf2factory" args="robot_description" respawn="false" output="screen"/>
+ <node pkg="gazebo_tools" type="urdf2factory" args="robot_description" respawn="false" output="screen"/>
<!-- start arm controller -->
<!--
Modified: pkg/trunk/demos/arm_gazebo/r_arm_grasping_demo.launch
===================================================================
--- pkg/trunk/demos/arm_gazebo/r_arm_grasping_demo.launch 2009-08-02 20:44:59 UTC (rev 20443)
+++ pkg/trunk/demos/arm_gazebo/r_arm_grasping_demo.launch 2009-08-02 21:02:43 UTC (rev 20444)
@@ -12,7 +12,7 @@
<param name="robot_description" command="$(find xacro)/xacro.py '$(find pr2_defs)/robots/r_arm.xacro.xml'" />
<!-- push robot_description to factory and spawn robot in gazebo -->
- <node pkg="gazebo_plugin" type="urdf2factory" args="robot_description" respawn="false" output="screen"/>
+ <node pkg="gazebo_tools" type="urdf2factory" args="robot_description" respawn="false" output="screen"/>
<!-- start arm controller -->
<!--
@@ -39,7 +39,7 @@
<!--include file="$(find arm_gazebo)/tests/debug_plot.launch"/-->
<param name="table_description" command="$(find xacro)/xacro.py '$(find examples_gazebo)/table_defs/table_defs.xml'" />
- <node pkg="gazebo_plugin" type="urdf2factory" args="table_description 0 0 0.01 0 0 0 table_model" respawn="false" output="screen" />
+ <node pkg="gazebo_tools" type="urdf2factory" args="table_description 0 0 0.01 0 0 0 table_model" respawn="false" output="screen" />
<include file="$(find examples_gazebo)/coffee_cup.launch" />
</launch>
Modified: pkg/trunk/demos/arm_gazebo/r_gripper.launch
===================================================================
--- pkg/trunk/demos/arm_gazebo/r_gripper.launch 2009-08-02 20:44:59 UTC (rev 20443)
+++ pkg/trunk/demos/arm_gazebo/r_gripper.launch 2009-08-02 21:02:43 UTC (rev 20444)
@@ -4,7 +4,7 @@
<param name="robot_description" command="$(find xacro)/xacro.py '$(find pr2_defs)/robots/r_gripper.xacro.xml'" />
<!-- push robot_description to factory and spawn robot in gazebo -->
- <node pkg="gazebo_plugin" type="urdf2factory" args="robot_description" respawn="false" output="screen"/>
+ <node pkg="gazebo_tools" type="urdf2factory" args="robot_description" respawn="false" output="screen"/>
<!-- start arm controller -->
<!--
Modified: pkg/trunk/demos/arm_gazebo/scripts/test_launch_order.tcsh
===================================================================
--- pkg/trunk/demos/arm_gazebo/scripts/test_launch_order.tcsh 2009-08-02 20:44:59 UTC (rev 20443)
+++ pkg/trunk/demos/arm_gazebo/scripts/test_launch_order.tcsh 2009-08-02 21:02:43 UTC (rev 20444)
@@ -17,7 +17,7 @@
python ./setparam.py
echo "----------------urdf2factory"
-`rospack find gazebo_plugin`/urdf2factory "robot_description"
+`rospack find gazebo_tools`/urdf2factory "robot_description"
echo "----------------spawn controller"
`rospack find mechanism_control`/scripts/mech.py sp `rospack find arm_gazebo`/r_arm_default_controller.xml
Modified: pkg/trunk/demos/arm_gazebo/tests/r_arm_constraint.launch
===================================================================
--- pkg/trunk/demos/arm_gazebo/tests/r_arm_constraint.launch 2009-08-02 20:44:59 UTC (rev 20443)
+++ pkg/trunk/demos/arm_gazebo/tests/r_arm_constraint.launch 2009-08-02 21:02:43 UTC (rev 20444)
@@ -18,7 +18,7 @@
<param name="robot_description" command="$(find xacro)/xacro.py '$(find pr2_defs)/robots/r_arm.xacro.xml'" />
<!-- push robot_description to factory and spawn robot in gazebo -->
- <node pkg="gazebo_plugin" type="urdf2factory" args="robot_description" respawn="false" output="screen"/>
+ <node pkg="gazebo_tools" type="urdf2factory" args="robot_description" respawn="false" output="screen"/>
<!-- start arm controller -->
Modified: pkg/trunk/demos/arm_gazebo/tests/r_arm_spacenav.launch
===================================================================
--- pkg/trunk/demos/arm_gazebo/tests/r_arm_spacenav.launch 2009-08-02 20:44:59 UTC (rev 20443)
+++ pkg/trunk/demos/arm_gazebo/tests/r_arm_spacenav.launch 2009-08-02 21:02:43 UTC (rev 20444)
@@ -11,7 +11,7 @@
<param name="robot_description" command="$(find xacro)/xacro.py '$(find pr2_defs)/robots/r_arm.xacro.xml'" />
<!-- push robot_description to factory and spawn robot in gazebo -->
- <node pkg="gazebo_plugin" type="urdf2factory" args="robot_description" respawn="false" output="screen"/>
+ <node pkg="gazebo_tools" type="urdf2factory" args="robot_description" respawn="false" output="screen"/>
<!-- start arm controller -->
<!-- tried to revive, copied into arm_gazebo from arm_life_test, but controller breaks. -->
@@ -19,6 +19,6 @@
<node pkg="spacenav_node" type="spacenav_node" args="" respawn="false" output="screen" />
<param name="table_description" command="$(find xacro)/xacro.py '$(find examples_gazebo)/table_defs/table_defs.xml'" />
- <node pkg="gazebo_plugin" type="urdf2factory" args="table_description" respawn="false" output="screen" />
+ <node pkg="gazebo_tools" type="urdf2factory" args="table_description" respawn="false" output="screen" />
</launch>
Modified: pkg/trunk/demos/door_demos_gazebo/door_defs/Makefile
===================================================================
--- pkg/trunk/demos/door_demos_gazebo/door_defs/Makefile 2009-08-02 20:44:59 UTC (rev 20443)
+++ pkg/trunk/demos/door_demos_gazebo/door_defs/Makefile 2009-08-02 21:02:43 UTC (rev 20444)
@@ -1,5 +1,5 @@
XACRO = `rospack find xacro`/xacro.py
-URDF2GAZEBO = `rospack find gazebo_plugin`/urdf2file
+URDF2GAZEBO = `rospack find gazebo_tools`/urdf2file
ROBOT_PATH = `rospack find door_demos_gazebo`/robots
ROBOT = door
Modified: pkg/trunk/demos/door_demos_gazebo/launch/door_only_test.launch
===================================================================
--- pkg/trunk/demos/door_demos_gazebo/launch/door_only_test.launch 2009-08-02 20:44:59 UTC (rev 20443)
+++ pkg/trunk/demos/door_demos_gazebo/launch/door_only_test.launch 2009-08-02 21:02:43 UTC (rev 20444)
@@ -6,7 +6,7 @@
<param name="door_description" command="$(find xacro)/xacro.py '$(find door_demos_gazebo)/robots/door_only.xacro.xml'" />
<!-- push door_description to factory and spawn robot in gazebo -->
- <node pkg="gazebo_plugin" type="urdf2factory" args="door_description" respawn="false" output="screen" /> <!-- load default arm controller -->
+ <node pkg="gazebo_tools" type="urdf2factory" args="door_description" respawn="false" output="screen" /> <!-- load default arm controller -->
<!-- load door controllers -->
<node pkg="mechanism_control" type="spawner.py" args="$(find door_demos_gazebo)/door_defs/door_controllers.xml" respawn="false" output="screen" />
Modified: pkg/trunk/demos/door_demos_gazebo/launch/pr2_and_door.launch
===================================================================
--- pkg/trunk/demos/door_demos_gazebo/launch/pr2_and_door.launch 2009-08-02 20:44:59 UTC (rev 20443)
+++ pkg/trunk/demos/door_demos_gazebo/launch/pr2_and_door.launch 2009-08-02 21:02:43 UTC (rev 20444)
@@ -5,7 +5,7 @@
<!-- load pr2 -->
<param name="robot_description" command="$(find xacro)/xacro.py '$(find pr2_defs)/robots/pr2.xacro.xml'" />
- <node pkg="gazebo_plugin" type="urdf2factory" args="robot_description" respawn="false" output="screen" />
+ <node pkg="gazebo_tools" type="urdf2factory" args="robot_description" respawn="false" output="screen" />
<!-- Robot state publisher -->
<node pkg="robot_state_publisher" type="state_publisher" name="robot_state_publisher">
@@ -16,7 +16,7 @@
<!-- load door -->
<param name="door_description" command="$(find xacro)/xacro.py '$(find door_demos_gazebo)/door_defs/door_defs.xml'" />
- <node pkg="gazebo_plugin" type="urdf2factory" args="door_description 0 0 0 0 0 0 doorbot" respawn="false" output="screen" />
+ <node pkg="gazebo_tools" type="urdf2factory" args="door_description 0 0 0 0 0 0 doorbot" respawn="false" output="screen" />
<!-- load door and handle controllers
<node pkg="mechanism_control" type="spawner.py" args="$(find door_demos_gazebo)/door_defs/door_controllers.xml" respawn="false" output="screen" />
Modified: pkg/trunk/demos/erratic_gazebo/erratic_demo.launch
===================================================================
--- pkg/trunk/demos/erratic_gazebo/erratic_demo.launch 2009-08-02 20:44:59 UTC (rev 20443)
+++ pkg/trunk/demos/erratic_gazebo/erratic_demo.launch 2009-08-02 21:02:43 UTC (rev 20444)
@@ -16,7 +16,7 @@
</node>
<!-- push robot_description to factory and spawn robot in gazebo -->
- <node pkg="gazebo_plugin" type="urdf2factory" args="robot_description" respawn="false" output="screen" />
+ <node pkg="gazebo_tools" type="urdf2factory" args="robot_description" respawn="false" output="screen" />
<!-- load controllers -->
<node pkg="gazebo_plugin" type="ros_diffdrive" respawn="true" output="screen"/>
Modified: pkg/trunk/demos/examples_gazebo/coffee_cup.launch
===================================================================
--- pkg/trunk/demos/examples_gazebo/coffee_cup.launch 2009-08-02 20:44:59 UTC (rev 20443)
+++ pkg/trunk/demos/examples_gazebo/coffee_cup.launch 2009-08-02 21:02:43 UTC (rev 20444)
@@ -4,7 +4,7 @@
<param name="coffee_cup" textfile="$(find gazebo_worlds)/objects/coffee_cup.model" />
<!-- push urdf to factory and spawn robot in gazebo -->
- <node pkg="gazebo_plugin" type="xml2factory" args="coffee_cup 1 -0.22 .84 0 0 0 coffee_cup" respawn="false" output="screen" />
+ <node pkg="gazebo_tools" type="xml2factory" args="coffee_cup 1 -0.22 .84 0 0 0 coffee_cup" respawn="false" output="screen" />
</launch>
Modified: pkg/trunk/demos/examples_gazebo/cups.launch
===================================================================
--- pkg/trunk/demos/examples_gazebo/cups.launch 2009-08-02 20:44:59 UTC (rev 20443)
+++ pkg/trunk/demos/examples_gazebo/cups.launch 2009-08-02 21:02:43 UTC (rev 20444)
@@ -7,10 +7,10 @@
<param name="coffee_cup" textfile="$(find gazebo_worlds)/objects/coffee_cup.model" />
<!-- push urdf to factory and spawn robot in gazebo -->
- <node pkg="gazebo_plugin" type="xml2factory" args="ikea_cup 0 -.8 0 0 0 0 ikea_cup" respawn="false" output="screen" />
- <node pkg="gazebo_plugin" type="xml2factory" args="wine 1 0.0 0 0 0 0 wine" respawn="false" output="screen" />
- <node pkg="gazebo_plugin" type="xml2factory" args="bowl 0 0.0 0 0 0 0 bowl" respawn="false" output="screen" />
- <node pkg="gazebo_plugin" type="xml2factory" args="coffee_cup 0 .8 0 0 0 0 coffee_cup" respawn="false" output="screen" />
+ <node pkg="gazebo_tools" type="xml2factory" args="ikea_cup 0 -.8 0 0 0 0 ikea_cup" respawn="false" output="screen" />
+ <node pkg="gazebo_tools" type="xml2factory" args="wine 1 0.0 0 0 0 0 wine" respawn="false" output="screen" />
+ <node pkg="gazebo_tools" type="xml2factory" args="bowl 0 0.0 0 0 0 0 bowl" respawn="false" output="screen" />
+ <node pkg="gazebo_tools" type="xml2factory" args="coffee_cup 0 .8 0 0 0 0 coffee_cup" respawn="false" output="screen" />
</launch>
Modified: pkg/trunk/demos/examples_gazebo/cups2.launch
===================================================================
--- pkg/trunk/demos/examples_gazebo/cups2.launch 2009-08-02 20:44:59 UTC (rev 20443)
+++ pkg/trunk/demos/examples_gazebo/cups2.launch 2009-08-02 21:02:43 UTC (rev 20444)
@@ -6,10 +6,10 @@
<param name="coffee_cup" textfile="$(find gazebo_worlds)/objects/coffee_cup.model" />
<!-- push urdf to factory and spawn robot in gazebo -->
- <node pkg="gazebo_plugin" type="xml2factory" args="ikea_cup 0 -.8 0 0 0 0 ikea_cup" respawn="false" output="screen" />
- <node pkg="gazebo_plugin" type="xml2factory" args="wine 2 0.0 0 0 0 0 wine" respawn="false" output="screen" />
- <node pkg="gazebo_plugin" type="xml2factory" args="bowl 0 0.0 0 0 0 0 bowl" respawn="false" output="screen" />
- <node pkg="gazebo_plugin" type="xml2factory" args="coffee_cup 0 .8 0 0 0 0 coffee_cup" respawn="false" output="screen" />
+ <node pkg="gazebo_tools" type="xml2factory" args="ikea_cup 0 -.8 0 0 0 0 ikea_cup" respawn="false" output="screen" />
+ <node pkg="gazebo_tools" type="xml2factory" args="wine 2 0.0 0 0 0 0 wine" respawn="false" output="screen" />
+ <node pkg="gazebo_tools" type="xml2factory" args="bowl 0 0.0 0 0 0 0 bowl" respawn="false" output="screen" />
+ <node pkg="gazebo_tools" type="xml2factory" args="coffee_cup 0 .8 0 0 0 0 coffee_cup" respawn="false" output="screen" />
</launch>
Modified: pkg/trunk/demos/examples_gazebo/dual_link.launch
===================================================================
--- pkg/trunk/demos/examples_gazebo/dual_link.launch 2009-08-02 20:44:59 UTC (rev 20443)
+++ pkg/trunk/demos/examples_gazebo/dual_link.launch 2009-08-02 21:02:43 UTC (rev 20444)
@@ -3,7 +3,7 @@
<param name="robot_description" command="$(find xacro)/xacro.py '$(find examples_gazebo)/dual_link_defs/dual_link.xml'" />
<!-- push robot_description to factory and spawn robot in gazebo -->
- <node pkg="gazebo_plugin" type="urdf2factory" args="robot_description" respawn="false" output="screen" />
+ <node pkg="gazebo_tools" type="urdf2factory" args="robot_description" respawn="false" output="screen" />
<!--node pkg="mechanism_control" type="mech.py" args="sp $(find examples_gazebo)/dual_link_defs/controllers_dual_link.xml" respawn="false" output="screen" /-->
<!--node pkg="robot_mechanism_controllers" type="control.py" args="set test_controller 0.5" respawn="false" output="screen" /--> <!-- open gripper .5 radians -->
Modified: pkg/trunk/demos/examples_gazebo/dual_link_defs/Makefile
===================================================================
--- pkg/trunk/demos/examples_gazebo/dual_link_defs/Makefile 2009-08-02 20:44:59 UTC (rev 20443)
+++ pkg/trunk/demos/examples_gazebo/dual_link_defs/Makefile 2009-08-02 21:02:43 UTC (rev 20444)
@@ -1,5 +1,5 @@
XACRO = `rospack find xacro`/xacro.py
-URDF2GAZEBO = `rospack find gazebo_plugin`/bin/urdf2file
+URDF2GAZEBO = `rospack find gazebo_tools`/bin/urdf2file
MODEL_FILE = `rospack find examples_gazebo`
ROBOT = dual_link
Modified: pkg/trunk/demos/examples_gazebo/multi_link.launch
===================================================================
--- pkg/trunk/demos/examples_gazebo/multi_link.launch 2009-08-02 20:44:59 UTC (rev 20443)
+++ pkg/trunk/demos/examples_gazebo/multi_link.launch 2009-08-02 21:02:43 UTC (rev 20444)
@@ -3,7 +3,7 @@
<param name="robot_description" command="$(find xacro)/xacro.py '$(find examples_gazebo)/multi_link_defs/multi_link.xml'" />
<!-- push robot_description to factory and spawn robot in gazebo -->
- <node pkg="gazebo_plugin" type="urdf2factory" args="robot_description" respawn="false" output="screen" />
+ <node pkg="gazebo_tools" type="urdf2factory" args="robot_description" respawn="false" output="screen" />
<node pkg="mechanism_control" type="mech.py" args="sp $(find examples_gazebo)/multi_link_defs/controllers_multi_link_zero_gains.xml" respawn="false" output="screen" />
<!--node pkg="robot_mechanism_controllers" type="control.py" args="set test_controller 0.5" respawn="false" output="screen" /--> <!-- open gripper .5 radians -->
Modified: pkg/trunk/demos/examples_gazebo/multi_link_defs/Makefile
===================================================================
--- pkg/trunk/demos/examples_gazebo/multi_link_defs/Makefile 2009-08-02 20:44:59 UTC (rev 20443)
+++ pkg/trunk/demos/examples_gazebo/multi_link_defs/Makefile 2009-08-02 21:02:43 UTC (rev 20444)
@@ -1,5 +1,5 @@
XACRO = `rospack find xacro`/xacro.py
-URDF2GAZEBO = `rospack find gazebo_plugin`/bin/urdf2file
+URDF2GAZEBO = `rospack find gazebo_tools`/bin/urdf2file
MODEL_FILE = `rospack find examples_gazebo`
ROBOT = multi_link
Modified: pkg/trunk/demos/examples_gazebo/simple_box.launch
===================================================================
--- pkg/trunk/demos/examples_gazebo/simple_box.launch 2009-08-02 20:44:59 UTC (rev 20443)
+++ pkg/trunk/demos/examples_gazebo/simple_box.launch 2009-08-02 21:02:43 UTC (rev 20444)
@@ -1,6 +1,6 @@
<launch>
<!-- send simple_box.xml to param server -->
<param name="simple_box" command="$(find xacro)/xacro.py '$(find examples_gazebo)/simple_rdf_examples/simple_box.xml'" />
- <node pkg="gazebo_plugin" type="urdf2factory" args="simple_box" respawn="false" output="screen" />
+ <node pkg="gazebo_tools" type="urdf2factory" args="simple_box" respawn="false" output="screen" />
</launch>
Modified: pkg/trunk/demos/examples_gazebo/simple_cylinder.launch
===================================================================
--- pkg/trunk/demos/examples_gazebo/simple_cylinder.launch 2009-08-02 20:44:59 UTC (rev 20443)
+++ pkg/trunk/demos/examples_gazebo/simple_cylinder.launch 2009-08-02 21:02:43 UTC (rev 20444)
@@ -1,6 +1,6 @@
<launch>
<!-- send simple_cylinder.xml to param server -->
<param name="simple_cylinder" command="$(find xacro)/xacro.py '$(find examples_gazebo)/simple_rdf_examples/simple_cylinder.xml'" />
- <node pkg="gazebo_plugin" type="urdf2factory" args="simple_cylinder" respawn="false" output="screen" />
+ <node pkg="gazebo_tools" type="urdf2factory" args="simple_cylinder" respawn="false" output="screen" />
</launch>
Modified: pkg/trunk/demos/examples_gazebo/simple_sphere.launch
===================================================================
--- pkg/trunk/demos/examples_gazebo/simple_sphere.launch 2009-08-02 20:44:59 UTC (rev 20443)
+++ pkg/trunk/demos/examples_gazebo/simple_sphere.launch 2009-08-02 21:02:43 UTC (rev 20444)
@@ -1,6 +1,6 @@
<launch>
<!-- send simple_sphere.xml to param server -->
<param name="simple_sphere" command="$(find xacro)/xacro.py '$(find examples_gazebo)/simple_rdf_examples/simple_sphere.xml'" />
- <node pkg="gazebo_plugin" type="urdf2factory" args="simple_sphere" respawn="false" output="screen" />
+ <node pkg="gazebo_tools" type="urdf2factory" args="simple_sphere" respawn="false" output="screen" />
</launch>
Modified: pkg/trunk/demos/examples_gazebo/single_link.launch
===================================================================
--- pkg/trunk/demos/examples_gazebo/single_link.launch 2009-08-02 20:44:59 UTC (rev 20443)
+++ pkg/trunk/demos/examples_gazebo/single_link.launch 2009-08-02 21:02:43 UTC (rev 20444)
@@ -3,7 +3,7 @@
<param name="robot_description" command="$(find xacro)/xacro.py '$(find examples_gazebo)/single_link_defs/single_link.xml'" />
<!-- push robot_description to factory and spawn robot in gazebo -->
- <node pkg="gazebo_plugin" type="urdf2factory" args="robot_description" respawn="false" output="screen" />
+ <node pkg="gazebo_tools" type="urdf2factory" args="robot_description" respawn="false" output="screen" />
<node pkg="mechanism_control" type="spawner.py" args="$(find examples_gazebo)/single_link_defs/controllers_single_link.xml" respawn="false" output="screen" />
Modified: pkg/trunk/demos/examples_gazebo/single_link_defs/Makefile
===================================================================
--- pkg/trunk/demos/examples_gazebo/single_link_defs/Makefile 2009-08-02 20:44:59 UTC (rev 20443)
+++ pkg/trunk/demos/examples_gazebo/single_link_defs/Makefile 2009-08-02 21:02:43 UTC (rev 20444)
@@ -1,5 +1,5 @@
XACRO = `rospack find xacro`/xacro.py
-URDF2GAZEBO = `rospack find gazebo_plugin`/bin/urdf2file
+URDF2GAZEBO = `rospack find gazebo_tools`/bin/urdf2file
MODEL_FILE = `rospack find examples_gazebo`
ROBOT = single_link
Modified: pkg/trunk/demos/examples_gazebo/table.launch
===================================================================
--- pkg/trunk/demos/examples_gazebo/table.launch 2009-08-02 20:44:59 UTC (rev 20443)
+++ pkg/trunk/demos/examples_gazebo/table.launch 2009-08-02 21:02:43 UTC (rev 20444)
@@ -3,6 +3,6 @@
<param name="table_description" command="$(find xacro)/xacro.py '$(find examples_gazebo)/table_defs/table.xml'" />
<!-- push table_description to factory and spawn robot in gazebo -->
- <node pkg="gazebo_plugin" type="urdf2factory" args="table_description" respawn="false" output="screen" /> <!-- load default arm controller -->
+ <node pkg="gazebo_tools" type="urdf2factory" args="table_description" respawn="false" output="screen" /> <!-- load default arm controller -->
</launch>
Modified: pkg/trunk/demos/examples_gazebo/table_defs/Makefile
===================================================================
--- pkg/trunk/demos/examples_gazebo/table_defs/Makefile 2009-08-02 20:44:59 UTC (rev 20443)
+++ pkg/trunk/demos/examples_gazebo/table_defs/Makefile 2009-08-02 21:02:43 UTC (rev 20444)
@@ -1,5 +1,5 @@
XACRO = `rospack find xacro`/xacro.py
-URDF2GAZEBO = `rospack find gazebo_plugin`/bin/urdf2file
+URDF2GAZEBO = `rospack find gazebo_tools`/bin/urdf2file
MODEL_FILE = `rospack find examples_gazebo`
ROBOT = table
Modified: pkg/trunk/demos/examples_gazebo/tables.launch
===================================================================
--- pkg/trunk/demos/examples_gazebo/tables.launch 2009-08-02 20:44:59 UTC (rev 20443)
+++ pkg/trunk/demos/examples_gazebo/tables.launch 2009-08-02 21:02:43 UTC (rev 20444)
@@ -3,8 +3,8 @@
<param name="desk1_model" textfile="$(find gazebo_worlds)/objects/desk1.model" />
<param name="desk2_model" textfile="$(find gazebo_worlds)/objects/desk2.model" />
<param name="desk3_model" textfile="$(find gazebo_worlds)/objects/desk3.model" />
- <node pkg="gazebo_plugin" type="xml2factory" args="desk1_model -2.0 -12.0 0.0 0 0 0 desk1_model" respawn="false" output="screen" />
- <node pkg="gazebo_plugin" type="xml2factory" args="desk2_model -11.0 2.5 0.0 0 0 0 desk2_model" respawn="false" output="screen" />
- <node pkg="gazebo_plugin" type="xml2factory" args="desk3_model -4.0 -12.0 0.0 0 0 0 desk3_model" respawn="false" output="screen" />
+ <node pkg="gazebo_tools" type="xml2factory" args="desk1_model -2.0 -12.0 0.0 0 0 0 desk1_model" respawn="false" output="screen" />
+ <node pkg="gazebo_tools" type="xml2factory" args="desk2_model -11.0 2.5 0.0 0 0 0 desk2_model" respawn="false" output="screen" />
+ <node pkg="gazebo_tools" type="xml2factory" args="desk3_model -4.0 -12.0 0.0 0 0 0 desk3_model" respawn="false" output="screen" />
</launch>
Modified: pkg/trunk/demos/plug_in_gazebo/launch/plug_outlet.launch
===================================================================
--- pkg/trunk/demos/plug_in_gazebo/launch/plug_outlet.launch 2009-08-02 20:44:59 UTC (rev 20443)
+++ pkg/trunk/demos/plug_in_gazebo/launch/plug_outlet.launch 2009-08-02 21:02:43 UTC (rev 20444)
@@ -4,9 +4,9 @@
<!-- send plug_only.xml to param server and spawn robot-->
<param name="plug_description" command="$(find xacro)/xacro.py '$(find plug_in_gazebo)/robots/plug_only.xacro.xml'" />
- <node pkg="gazebo_plugin" type="urdf2factory" args="plug_description 0 0 0 0 0 0 plug" respawn="false" output="screen" />
+ <node pkg="gazebo_tools" type="urdf2factory" args="plug_description 0 0 0 0 0 0 plug" respawn="false" output="screen" />
<param name="outlet_description" command="$(find xacro)/xacro.py '$(find plug_in_gazebo)/robots/outlet_only.xacro.xml'" />
- <node pkg="gazebo_plugin" type="urdf2factory" args="outlet_description" respawn="false" output="screen" />
+ <node pkg="gazebo_tools" type="urdf2factory" args="outlet_description" respawn="false" output="screen" />
</launch>
Modified: pkg/trunk/demos/plug_in_gazebo/launch/pr2_plug_outlet.launch
===================================================================
--- pkg/trunk/demos/plug_in_gazebo/launch/pr2_plug_outlet.launch 2009-08-02 20:44:59 UTC (rev 20443)
+++ pkg/trunk/demos/plug_in_gazebo/launch/pr2_plug_outlet.launch 2009-08-02 21:02:43 UTC (rev 20444)
@@ -5,13 +5,13 @@
<!-- send pr2.xml to param server -->
<!-- push urdfs to factory and spawn robot in gazebo -->
<param ns="pr2" name="robot_description" command="$(find xacro)/xacro.py '$(find pr2_defs)/robots/prf.xacro.xml'" />
- <node pkg="gazebo_plugin" type="urdf2factory" args="robot_description" respawn="false" output="screen" />
+ <node pkg="gazebo_tools" type="urdf2factory" args="robot_description" respawn="false" output="screen" />
<param name="plug_description" command="$(find xacro)/xacro.py '$(find plug_in_gazebo)/robots/plug_only.xacro.xml'" />
- <node pkg="gazebo_plugin" type="urdf2factory" args="plug_description 0 0 0 0 0 0 plug" respawn="false" output="screen" />
+ <node pkg="gazebo_tools" type="urdf2factory" args="plug_description 0 0 0 0...
[truncated message content] |
|
From: <hsu...@us...> - 2009-08-02 21:55:37
|
Revision: 20448
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=20448&view=rev
Author: hsujohnhsu
Date: 2009-08-02 21:55:27 +0000 (Sun, 02 Aug 2009)
Log Message:
-----------
update controller launch scripts.
update gazebo test mechanism control.
Modified Paths:
--------------
pkg/trunk/demos/pr2_gazebo/map_building_demo.launch
pkg/trunk/demos/pr2_gazebo/multi_level_map_demo.launch
pkg/trunk/demos/pr2_gazebo/pr2_default_controllers.launch
pkg/trunk/demos/pr2_gazebo/pr2_trajectory_controllers.launch
pkg/trunk/demos/pr2_gazebo/prototype1_default_controllers.launch
pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/hztest_pr2_mechanism.launch
pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/hztest_pr2_odom.launch
pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/hztest_rostime.launch
pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/my_hztest.launch
pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/test_arm.launch
pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/test_base.launch
Added Paths:
-----------
pkg/trunk/demos/arm_gazebo/controllers/l_arm_position_controller.launch
pkg/trunk/demos/arm_gazebo/controllers/r_arm_position_controller.launch
Added: pkg/trunk/demos/arm_gazebo/controllers/l_arm_position_controller.launch
===================================================================
--- pkg/trunk/demos/arm_gazebo/controllers/l_arm_position_controller.launch (rev 0)
+++ pkg/trunk/demos/arm_gazebo/controllers/l_arm_position_controller.launch 2009-08-02 21:55:27 UTC (rev 20448)
@@ -0,0 +1,6 @@
+<launch>
+ <!-- arm controller -->
+ <rosparam file="$(find arm_gazebo)/controllers/l_arm_position_controller.xml" command="load" output="screen"/>
+ <node pkg="mechanism_control" type="spawner.py" args="l_shoulder_pan_controller l_shoulder_lift_controller l_upper_arm_roll_controller l_elbow_flex_controller l_forearm_roll_controller l_wrist_flex_controller l_wrist_roll_controller l_gripper_controller" output="screen"/>
+</launch>
+
Property changes on: pkg/trunk/demos/arm_gazebo/controllers/l_arm_position_controller.launch
___________________________________________________________________
Added: svn:mergeinfo
+
Added: pkg/trunk/demos/arm_gazebo/controllers/r_arm_position_controller.launch
===================================================================
--- pkg/trunk/demos/arm_gazebo/controllers/r_arm_position_controller.launch (rev 0)
+++ pkg/trunk/demos/arm_gazebo/controllers/r_arm_position_controller.launch 2009-08-02 21:55:27 UTC (rev 20448)
@@ -0,0 +1,5 @@
+<launch>
+ <!-- arm controller -->
+ <rosparam file="$(find arm_gazebo)/controllers/r_arm_position_controller.xml" command="load" output="screen"/>
+ <node pkg="mechanism_control" type="spawner.py" args="r_shoulder_pan_controller r_shoulder_lift_controller r_upper_arm_roll_controller r_elbow_flex_controller r_forearm_roll_controller r_wrist_flex_controller r_wrist_roll_controller r_gripper_controller" output="screen"/>
+
Property changes on: pkg/trunk/demos/arm_gazebo/controllers/r_arm_position_controller.launch
___________________________________________________________________
Added: svn:mergeinfo
+
Modified: pkg/trunk/demos/pr2_gazebo/map_building_demo.launch
===================================================================
--- pkg/trunk/demos/pr2_gazebo/map_building_demo.launch 2009-08-02 21:46:29 UTC (rev 20447)
+++ pkg/trunk/demos/pr2_gazebo/map_building_demo.launch 2009-08-02 21:55:27 UTC (rev 20448)
@@ -6,10 +6,9 @@
<!-- start prototype1 robot (pr2 - arms) -->
<include file="$(find pr2_gazebo)/prototype1.launch"/>
- <!-- startup base controller -->
- <param name="base_controller/odom_publish_rate" value="10" />
- <node pkg="mechanism_control" type="spawner.py" args="$(find pr2_default_controllers)/base_controller.xml" output="screen"/>
- <node pkg="mechanism_control" type="spawner.py" args="$(find pr2_default_controllers)/gazebo_head_torso_lift_controller.xml" output="screen"/>
+ <!-- load head and base controllers -->
+ <include file="$(find pr2_default_controllers)/pr2_base_controller_odom.launch"/>
+ <include file="$(find pr2_default_controllers)/head_position_controller.launch"/>
<!-- for visualization -->
<node pkg="rviz" type="rviz" respawn="false" />
Modified: pkg/trunk/demos/pr2_gazebo/multi_level_map_demo.launch
===================================================================
--- pkg/trunk/demos/pr2_gazebo/multi_level_map_demo.launch 2009-08-02 21:46:29 UTC (rev 20447)
+++ pkg/trunk/demos/pr2_gazebo/multi_level_map_demo.launch 2009-08-02 21:55:27 UTC (rev 20448)
@@ -5,9 +5,9 @@
<!-- start prototype1 robot (pr2 - arms) -->
<include file="$(find pr2_gazebo)/prototype1.launch"/>
- <!-- startup base controller -->
- <param name="base_controller/odom_publish_rate" value="10" />
- <node pkg="mechanism_control" type="spawner.py" args="$(find pr2_default_controllers)/base_controller.xml" output="screen"/>
- <node pkg="mechanism_control" type="spawner.py" args="$(find pr2_default_controllers)/gazebo_head_torso_lift_controller.xml" output="screen"/>
+ <!-- load head and base controllers -->
+ <include file="$(find pr2_default_controllers)/pr2_base_controller_odom.launch"/>
+ <include file="$(find pr2_default_controllers)/head_position_controller.launch"/>
+
</launch>
Modified: pkg/trunk/demos/pr2_gazebo/pr2_default_controllers.launch
===================================================================
--- pkg/trunk/demos/pr2_gazebo/pr2_default_controllers.launch 2009-08-02 21:46:29 UTC (rev 20447)
+++ pkg/trunk/demos/pr2_gazebo/pr2_default_controllers.launch 2009-08-02 21:55:27 UTC (rev 20448)
@@ -1,23 +1,11 @@
<launch>
- <param name="base_controller/odom_publish_rate" value="10" />
- <!--node pkg="mechanism_control" type="mech.py" args="sp $(find pr2_gazebo)/controllers_2dnav_test.xml" respawn="false" output="screen" /-->
- <node pkg="mechanism_control" type="spawner.py" args="$(find pr2_default_controllers)/base_controller.xml" output="screen"/>
- <node pkg="mechanism_control" type="spawner.py" args="$(find pr2_default_controllers)/gazebo_head_torso_lift_controller.xml" output="screen"/>
- <!-- arm controller -->
- <node pkg="mechanism_control" type="spawner.py" args="$(find arm_gazebo)/controllers/l_arm_position_controller.xml" output="screen"/>
- <node pkg="mechanism_control" type="spawner.py" args="$(find arm_gazebo)/controllers/r_arm_position_controller.xml" output="screen"/>
+ <!-- load controllers -->
+ <include file="$(find pr2_default_controllers)/pr2_base_controller_odom.launch"/>
+ <include file="$(find pr2_default_controllers)/head_position_controller.launch"/>
+ <include file="$(find pr2_gazebo)/controllers/pr2_tilt_laser_controller.launch"/>
+ <include file="$(find arm_gazebo)/controllers/l_arm_position_controller.launch" output="screen"/>
+ <include file="$(find arm_gazebo)/controllers/r_arm_position_controller.launch" output="screen"/>
- <!-- Tug Arms For Navigation -->
- <!--node pkg="pr2_mechanism_controllers" type="tuckarm.py" arg="b" output="screen"/-->
- <!-- PR2 Calibration -->
- <!--include file="$(find pr2_default_controllers)/prototype1_calibration_controller.launch" /-->
-
- <!-- send laser tilt motor a command -->
- <!--node pkg="pr2_mechanism_controllers" type="send_periodic_cmd_srv.py" args="laser_tilt_controller linear 20 0.872 0.3475" respawn="false" output="screen" /-->
- <!--node pkg="pr2_mechanism_controllers" type="send_periodic_cmd_srv.py" args="laser_tilt_controller linear 1 .45 .40" /--> <!-- this one will pick up shoulders at lowest position -->
- <node pkg="mechanism_control" type="spawner.py" args="$(find pr2_default_controllers)/laser_tilt_controller.xml" />
- <node pkg="pr2_mechanism_controllers" type="send_periodic_cmd_srv.py" args="laser_tilt_controller linear 2 .45 .40" />
-
</launch>
Modified: pkg/trunk/demos/pr2_gazebo/pr2_trajectory_controllers.launch
===================================================================
--- pkg/trunk/demos/pr2_gazebo/pr2_trajectory_controllers.launch 2009-08-02 21:46:29 UTC (rev 20447)
+++ pkg/trunk/demos/pr2_gazebo/pr2_trajectory_controllers.launch 2009-08-02 21:55:27 UTC (rev 20448)
@@ -6,13 +6,12 @@
<param name="base_trajectory_controller/controller_frequency" value="20.0" type="double" />
<node pkg="pr2_mechanism_controllers" type="base_trajectory_controller"/>
- <!-- base odom controller -->
- <param name="base_controller/odom_publish_rate" value="10" />
- <node pkg="mechanism_control" type="spawner.py" args="$(find pr2_default_controllers)/base_controller.xml" output="screen"/>
+ <!-- load head and base controllers -->
+ <include file="$(find pr2_default_controllers)/pr2_base_controller_odom.launch"/>
+ <include file="$(find pr2_default_controllers)/head_position_controller.launch"/>
<!-- tilting laser controller -->
- <node pkg="mechanism_control" type="spawner.py" args="$(find pr2_default_controllers)/laser_tilt_controller.xml" />
- <node pkg="pr2_mechanism_controllers" type="send_periodic_cmd_srv.py" args="laser_tilt_controller linear 2 .45 .40" />
+ <include file="$(find pr2_gazebo)/controllers/pr2_tilt_laser_controller.launch"/>
</launch>
Modified: pkg/trunk/demos/pr2_gazebo/prototype1_default_controllers.launch
===================================================================
--- pkg/trunk/demos/pr2_gazebo/prototype1_default_controllers.launch 2009-08-02 21:46:29 UTC (rev 20447)
+++ pkg/trunk/demos/pr2_gazebo/prototype1_default_controllers.launch 2009-08-02 21:55:27 UTC (rev 20448)
@@ -1,13 +1,9 @@
<launch>
- <!-- use spawner.py to spawn all controllers listed in controllers.xml -->
- <param name="base_controller/odom_publish_rate" value="10" />
- <node pkg="mechanism_control" type="spawner.py" args="$(find pr2_default_controllers)/base_controller.xml" output="screen"/>
- <node pkg="mechanism_control" type="spawner.py" args="$(find pr2_default_controllers)/gazebo_head_torso_lift_controller.xml" output="screen"/>
- <!-- start tilting Hokuyo laser by sending it a preset code of 46, this means sawtooth profile sweep.
- for details of the profile, rates, see controller::LaserScannerControllerNode. -->
- <!--node pkg="pr2_mechanism_controllers" type="send_periodic_cmd_srv.py" args="laser_tilt_controller linear 20 0.872 0.3475" respawn="false" output="screen" /-->
- <node pkg="mechanism_control" type="spawner.py" args="$(find pr2_default_controllers)/laser_tilt_controller.xml" />
- <node pkg="pr2_mechanism_controllers" type="send_periodic_cmd_srv.py" args="laser_tilt_controller linear 2 .45 .40" />
+ <!-- load controllers -->
+ <include file="$(find pr2_default_controllers)/pr2_base_controller_odom.launch"/>
+ <include file="$(find pr2_default_controllers)/head_position_controller.launch"/>
+ <include file="$(find pr2_gazebo)/controllers/pr2_tilt_laser_controller.launch"/>
+
</launch>
Modified: pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/hztest_pr2_mechanism.launch
===================================================================
--- pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/hztest_pr2_mechanism.launch 2009-08-02 21:46:29 UTC (rev 20447)
+++ pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/hztest_pr2_mechanism.launch 2009-08-02 21:55:27 UTC (rev 20448)
@@ -6,17 +6,10 @@
<include file="$(find pr2_gazebo)/pr2.launch"/>
<!-- load controllers -->
+ <include file="$(find pr2_default_controllers)/pr2_base_controller_odom.launch"/>
+ <include file="$(find pr2_default_controllers)/head_position_controller.launch"/>
+ <include file="$(find pr2_gazebo)/controllers/pr2_tilt_laser_controller.launch"/>
- <rosparam file="$(find pr2_default_controllers)/pr2_base_controller.yaml" command="load" ns="pr2_base_controller" />
- <rosparam file="$(find pr2_default_controllers)/pr2_odometry.yaml" command="load" ns="pr2_odometry" />
- <node pkg="mechanism_control" type="spawner.py" args="$(find pr2_default_controllers)/pr2_base_controller.xml" output="screen"/>
- <node pkg="mechanism_control" type="spawner.py" args="$(find pr2_default_controllers)/pr2_base_odometry.xml" output="screen"/>
-
-
- <node pkg="mechanism_control" type="spawner.py" args="$(find pr2_default_controllers)/gazebo_head_torso_lift_controller.xml" output="screen"/>
- <node pkg="mechanism_control" type="spawner.py" args="$(find pr2_experimental_controllers)/laser_tilt/laser_tilt_controller.xml" />
- <node pkg="pr2_mechanism_controllers" type="send_laser_traj_cmd_ms2.py" args="laser_tilt_controller" />
-
<!-- Run hztest -->
<!-- Test for publication of 'base_pose_ground_truth' topic. -->
<test test-name="hztest_test_base_pose_ground_truth" pkg="rostest" type="hztest" time-limit="50" name="base_pose_ground_truth_test">
Modified: pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/hztest_pr2_odom.launch
===================================================================
--- pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/hztest_pr2_odom.launch 2009-08-02 21:46:29 UTC (rev 20447)
+++ pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/hztest_pr2_odom.launch 2009-08-02 21:55:27 UTC (rev 20448)
@@ -6,15 +6,10 @@
<include file="$(find pr2_gazebo)/pr2.launch"/>
<!-- load controllers -->
- <rosparam file="$(find pr2_default_controllers)/pr2_base_controller.yaml" command="load" ns="pr2_base_controller" />
- <rosparam file="$(find pr2_default_controllers)/pr2_odometry.yaml" command="load" ns="pr2_odometry" />
- <node pkg="mechanism_control" type="spawner.py" args="$(find pr2_default_controllers)/pr2_base_controller.xml" output="screen"/>
- <node pkg="mechanism_control" type="spawner.py" args="$(find pr2_default_controllers)/pr2_base_odometry.xml" output="screen"/>
+ <include file="$(find pr2_default_controllers)/pr2_base_controller_odom.launch"/>
+ <include file="$(find pr2_default_controllers)/head_position_controller.launch"/>
+ <include file="$(find pr2_gazebo)/controllers/pr2_tilt_laser_controller.launch"/>
- <node pkg="mechanism_control" type="spawner.py" args="$(find pr2_default_controllers)/gazebo_head_torso_lift_controller.xml" output="screen"/>
- <node pkg="mechanism_control" type="spawner.py" args="$(find pr2_experimental_controllers)/laser_tilt/laser_tilt_controller.xml" />
- <node pkg="pr2_mechanism_controllers" type="send_laser_traj_cmd_ms2.py" args="laser_tilt_controller" />
-
<!-- Test for publication of 'odom' topic. -->
<test test-name="hztest_test_odom" pkg="rostest" type="hztest" time-limit="50" name="odom_test">
<!-- Note how we use a different parameter namespace and node name
Modified: pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/hztest_rostime.launch
===================================================================
--- pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/hztest_rostime.launch 2009-08-02 21:46:29 UTC (rev 20447)
+++ pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/hztest_rostime.launch 2009-08-02 21:55:27 UTC (rev 20448)
@@ -6,16 +6,10 @@
<include file="$(find pr2_gazebo)/pr2.launch"/>
<!-- load controllers -->
- <rosparam file="$(find pr2_default_controllers)/pr2_base_controller.yaml" command="load" ns="pr2_base_controller" />
- <rosparam file="$(find pr2_default_controllers)/pr2_odometry.yaml" command="load" ns="pr2_odometry" />
- <node pkg="mechanism_control" type="spawner.py" args="$(find pr2_default_controllers)/pr2_base_controller.xml" output="screen"/>
- <node pkg="mechanism_control" type="spawner.py" args="$(find pr2_default_controllers)/pr2_base_odometry.xml" output="screen"/>
+ <include file="$(find pr2_default_controllers)/pr2_base_controller_odom.launch"/>
+ <include file="$(find pr2_default_controllers)/head_position_controller.launch"/>
+ <include file="$(find pr2_gazebo)/controllers/pr2_tilt_laser_controller.launch"/>
-
- <node pkg="mechanism_control" type="spawner.py" args="$(find pr2_default_controllers)/gazebo_head_torso_lift_controller.xml" output="screen"/>
- <node pkg="mechanism_control" type="spawner.py" args="$(find pr2_experimental_controllers)/laser_tilt/laser_tilt_controller.xml" />
- <node pkg="pr2_mechanism_controllers" type="send_laser_traj_cmd_ms2.py" args="laser_tilt_controller" />
-
<!-- Test for publication of 'odom' topic. -->
<test test-name="hztest_test_rostime" pkg="rostest" type="hztest" time-limit="50" name="rostime_test">
<!-- Note how we use a different parameter namespace and node name
Modified: pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/my_hztest.launch
===================================================================
--- pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/my_hztest.launch 2009-08-02 21:46:29 UTC (rev 20447)
+++ pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/my_hztest.launch 2009-08-02 21:55:27 UTC (rev 20448)
@@ -6,11 +6,9 @@
<include file="$(find pr2_gazebo)/pr2.launch"/>
<!-- load controllers -->
- <param name="base_controller/odom_publish_rate" value="10" />
- <node pkg="mechanism_control" type="spawner.py" args="$(find pr2_default_controllers)/base_controller.xml" output="screen"/>
- <node pkg="mechanism_control" type="spawner.py" args="$(find pr2_default_controllers)/gazebo_head_torso_lift_controller.xml" output="screen"/>
- <node pkg="mechanism_control" type="spawner.py" args="$(find pr2_experimental_controllers)/laser_tilt/laser_tilt_controller.xml" />
- <node pkg="pr2_mechanism_controllers" type="send_laser_traj_cmd_ms2.py" args="laser_tilt_controller" />
+ <include file="$(find pr2_default_controllers)/pr2_base_controller_odom.launch"/>
+ <include file="$(find pr2_default_controllers)/head_position_controller.launch"/>
+ <include file="$(find pr2_gazebo)/controllers/pr2_tilt_laser_controller.launch"/>
<!-- Run hztest -->
<test test-name="test_my_hztest_1" pkg="test_pr2_mechanism_controllers_gazebo" type="my_hztest.py" time-limit="100000000" />
Modified: pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/test_arm.launch
===================================================================
--- pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/test_arm.launch 2009-08-02 21:46:29 UTC (rev 20447)
+++ pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/test_arm.launch 2009-08-02 21:55:27 UTC (rev 20448)
@@ -5,8 +5,9 @@
<!-- send single_link.xml to param server -->
<include file="$(find arm_gazebo)/l_arm.launch" />
- <node pkg="pr2_default_controllers" type="tuckarm.py" args="l" respawn="true" />
- <node pkg="mechanism_control" type="spawner.py" args="$(find arm_gazebo)/controllers/l_gripper_controller.xml" respawn="true" />
+ <node pkg="pr2_default_controllers" type="tuckarm.py" args="l" respawn="true" />
+ <rosparam file="$(find arm_gazebo)/controllers/l_gripper_controller.xml" command="load" />
+ <node pkg="mechanism_control" type="spawner.py" args="l_gripper_controller" />
<test test-name="test_pr2_mechanism_gazebo_test_arm" pkg="test_pr2_mechanism_controllers_gazebo" type="test_arm.py" time-limit="110" />
</launch>
Modified: pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/test_base.launch
===================================================================
--- pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/test_base.launch 2009-08-02 21:46:29 UTC (rev 20447)
+++ pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/test_base.launch 2009-08-02 21:55:27 UTC (rev 20448)
@@ -6,15 +6,10 @@
<include file="$(find pr2_gazebo)/pr2.launch"/>
<!-- load controllers -->
- <rosparam file="$(find pr2_default_controllers)/pr2_base_controller.yaml" command="load" ns="pr2_base_controller" />
- <rosparam file="$(find pr2_default_controllers)/pr2_odometry.yaml" command="load" ns="pr2_odometry" />
- <node pkg="mechanism_control" type="spawner.py" args="$(find pr2_default_controllers)/pr2_base_controller.xml" output="screen"/>
- <node pkg="mechanism_control" type="spawner.py" args="$(find pr2_default_controllers)/pr2_base_odometry.xml" output="screen"/>
+ <include file="$(find pr2_default_controllers)/pr2_base_controller_odom.launch"/>
+ <include file="$(find pr2_default_controllers)/head_position_controller.launch"/>
+ <include file="$(find pr2_gazebo)/controllers/pr2_tilt_laser_controller.launch"/>
- <node pkg="mechanism_control" type="spawner.py" args="$(find pr2_default_controllers)/gazebo_head_torso_lift_controller.xml" output="screen"/>
- <node pkg="mechanism_control" type="spawner.py" args="$(find pr2_experimental_controllers)/laser_tilt/laser_tilt_controller.xml" />
- <node pkg="pr2_mechanism_controllers" type="send_laser_traj_cmd_ms2.py" args="laser_tilt_controller" />
-
<test test-name="test_pr2_mechanism_gazebo_test_base_vw1" pkg="test_pr2_mechanism_controllers_gazebo" type="test_base_vw_gt.py" />
<test test-name="test_pr2_mechanism_gazebo_test_base_odomxyw1" pkg="test_pr2_mechanism_controllers_gazebo" type="test_base_odomxyw_gt.py" />
<test test-name="test_pr2_mechanism_gazebo_test_base_odomxy1" pkg="test_pr2_mechanism_controllers_gazebo" type="test_base_odomxy_gt.py" />
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <is...@us...> - 2009-08-02 23:05:16
|
Revision: 20451
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=20451&view=rev
Author: isucan
Date: 2009-08-02 23:05:07 +0000 (Sun, 02 Aug 2009)
Log Message:
-----------
renamed service
Modified Paths:
--------------
pkg/trunk/highlevel/move_arm/src/arm_cmd_line.cpp
pkg/trunk/highlevel/move_arm/src/move_arm_action.cpp
pkg/trunk/motion_planning/ompl_planning/src/motion_planner.cpp
pkg/trunk/motion_planning/ompl_planning/src/request_handler/RequestHandler.cpp
pkg/trunk/motion_planning/ompl_planning/src/request_handler/RequestHandler.h
Added Paths:
-----------
pkg/trunk/motion_planning/motion_planning_msgs/srv/ConvertToJointConstraint.srv
Removed Paths:
-------------
pkg/trunk/motion_planning/motion_planning_msgs/srv/ConvertPoseToJointConstraint.srv
Modified: pkg/trunk/highlevel/move_arm/src/arm_cmd_line.cpp
===================================================================
--- pkg/trunk/highlevel/move_arm/src/arm_cmd_line.cpp 2009-08-02 22:57:21 UTC (rev 20450)
+++ pkg/trunk/highlevel/move_arm/src/arm_cmd_line.cpp 2009-08-02 23:05:07 UTC (rev 20451)
@@ -163,19 +163,19 @@
goal.goal_constraints.pose_constraint[0].pose.pose.orientation.z = pz[5];
goal.goal_constraints.pose_constraint[0].pose.pose.orientation.w = pz[6];
- goal.goal_constraints.pose_constraint[0].position_tolerance_above.x = 0.01;
- goal.goal_constraints.pose_constraint[0].position_tolerance_above.y = 0.01;
- goal.goal_constraints.pose_constraint[0].position_tolerance_above.z = 0.015;
- goal.goal_constraints.pose_constraint[0].position_tolerance_below.x = 0.01;
- goal.goal_constraints.pose_constraint[0].position_tolerance_below.y = 0.01;
- goal.goal_constraints.pose_constraint[0].position_tolerance_below.z = 0.015;
+ goal.goal_constraints.pose_constraint[0].position_tolerance_above.x = 0.001;
+ goal.goal_constraints.pose_constraint[0].position_tolerance_above.y = 0.001;
+ goal.goal_constraints.pose_constraint[0].position_tolerance_above.z = 0.002;
+ goal.goal_constraints.pose_constraint[0].position_tolerance_below.x = 0.001;
+ goal.goal_constraints.pose_constraint[0].position_tolerance_below.y = 0.001;
+ goal.goal_constraints.pose_constraint[0].position_tolerance_below.z = 0.002;
- goal.goal_constraints.pose_constraint[0].orientation_tolerance_above.x = 0.05;
- goal.goal_constraints.pose_constraint[0].orientation_tolerance_above.y = 0.05;
- goal.goal_constraints.pose_constraint[0].orientation_tolerance_above.z = 0.05;
- goal.goal_constraints.pose_constraint[0].orientation_tolerance_below.x = 0.05;
- goal.goal_constraints.pose_constraint[0].orientation_tolerance_below.y = 0.05;
- goal.goal_constraints.pose_constraint[0].orientation_tolerance_below.z = 0.05;
+ goal.goal_constraints.pose_constraint[0].orientation_tolerance_above.x = 0.005;
+ goal.goal_constraints.pose_constraint[0].orientation_tolerance_above.y = 0.005;
+ goal.goal_constraints.pose_constraint[0].orientation_tolerance_above.z = 0.005;
+ goal.goal_constraints.pose_constraint[0].orientation_tolerance_below.x = 0.005;
+ goal.goal_constraints.pose_constraint[0].orientation_tolerance_below.y = 0.005;
+ goal.goal_constraints.pose_constraint[0].orientation_tolerance_below.z = 0.005;
goal.goal_constraints.pose_constraint[0].orientation_importance = 0.01;
}
@@ -477,6 +477,7 @@
{
std::vector<double> params(response.solution.begin() + n, response.solution.begin() + n + u);
sp.setParamsJoint(params, names[i]);
+ std::cout << names[i] << " = " << params[0] << std::endl;
}
n += u;
}
Modified: pkg/trunk/highlevel/move_arm/src/move_arm_action.cpp
===================================================================
--- pkg/trunk/highlevel/move_arm/src/move_arm_action.cpp 2009-08-02 22:57:21 UTC (rev 20450)
+++ pkg/trunk/highlevel/move_arm/src/move_arm_action.cpp 2009-08-02 23:05:07 UTC (rev 20451)
@@ -42,8 +42,7 @@
#include <pr2_mechanism_controllers/TrajectoryCancel.h>
#include <manipulation_srvs/IKService.h>
-#include <manipulation_srvs/IKQuery.h>
-#include <motion_planning_msgs/ConvertPoseToJointConstraint.h>
+#include <motion_planning_msgs/ConvertToJointConstraint.h>
#include <visualization_msgs/Marker.h>
#include <cstdlib>
@@ -613,7 +612,7 @@
else
{
// if it is not, we try to fix it
- motion_planning_msgs::ConvertPoseToJointConstraint::Request c_req;
+ motion_planning_msgs::ConvertToJointConstraint::Request c_req;
c_req.params = req.params;
c_req.start_state = req.start_state;
c_req.constraints = req.goal_constraints;
@@ -641,8 +640,8 @@
}
}
- motion_planning_msgs::ConvertPoseToJointConstraint::Response c_res;
- ros::ServiceClient s_client = node_handle_.serviceClient<motion_planning_msgs::ConvertPoseToJointConstraint>(SEARCH_VALID_STATE_NAME);
+ motion_planning_msgs::ConvertToJointConstraint::Response c_res;
+ ros::ServiceClient s_client = node_handle_.serviceClient<motion_planning_msgs::ConvertToJointConstraint>(SEARCH_VALID_STATE_NAME);
if (s_client.call(c_req, c_res))
{
if (!c_res.joint_constraint.empty())
@@ -661,14 +660,14 @@
}
else
{
- motion_planning_msgs::ConvertPoseToJointConstraint::Request c_req;
+ motion_planning_msgs::ConvertToJointConstraint::Request c_req;
c_req.params = req.params;
c_req.start_state = req.start_state;
c_req.constraints = req.goal_constraints;
c_req.names = arm_joint_names_;
c_req.allowed_time = 1.0;
- motion_planning_msgs::ConvertPoseToJointConstraint::Response c_res;
- ros::ServiceClient s_client = node_handle_.serviceClient<motion_planning_msgs::ConvertPoseToJointConstraint>(SEARCH_VALID_STATE_NAME);
+ motion_planning_msgs::ConvertToJointConstraint::Response c_res;
+ ros::ServiceClient s_client = node_handle_.serviceClient<motion_planning_msgs::ConvertToJointConstraint>(SEARCH_VALID_STATE_NAME);
if (s_client.call(c_req, c_res))
{
if (!c_res.joint_constraint.empty())
Deleted: pkg/trunk/motion_planning/motion_planning_msgs/srv/ConvertPoseToJointConstraint.srv
===================================================================
--- pkg/trunk/motion_planning/motion_planning_msgs/srv/ConvertPoseToJointConstraint.srv 2009-08-02 22:57:21 UTC (rev 20450)
+++ pkg/trunk/motion_planning/motion_planning_msgs/srv/ConvertPoseToJointConstraint.srv 2009-08-02 23:05:07 UTC (rev 20451)
@@ -1,30 +0,0 @@
-# Parameters for the state space
-motion_planning_msgs/KinematicSpaceParameters params
-
-
-# Starting state updates. If certain joints should be considered
-# at positions other than the current ones, these positions should
-# be set here
-motion_planning_msgs/KinematicJoint[] start_state
-
-
-# The joint names, in the same order as the values in the state
-string[] names
-
-# A list of states, each with the dimension of the requested model.
-# The dimension of the model may be different from the number of joints
-# (when there are joints that use more than one parameter)
-# Each state is to be used as a hint (initialization) for the search
-# if the valid state
-motion_planning_msgs/KinematicState[] states
-
-# The input constraints, to be converted to a set of joint constraints
-motion_planning_msgs/KinematicConstraints constraints
-
-# The maximum amount of time the search is to be run for
-float64 allowed_time
-
----
-
-# The set of joint constraints that correspond to the pose constraint
-motion_planning_msgs/JointConstraint[] joint_constraint
Copied: pkg/trunk/motion_planning/motion_planning_msgs/srv/ConvertToJointConstraint.srv (from rev 20427, pkg/trunk/motion_planning/motion_planning_msgs/srv/ConvertPoseToJointConstraint.srv)
===================================================================
--- pkg/trunk/motion_planning/motion_planning_msgs/srv/ConvertToJointConstraint.srv (rev 0)
+++ pkg/trunk/motion_planning/motion_planning_msgs/srv/ConvertToJointConstraint.srv 2009-08-02 23:05:07 UTC (rev 20451)
@@ -0,0 +1,30 @@
+# Parameters for the state space
+motion_planning_msgs/KinematicSpaceParameters params
+
+
+# Starting state updates. If certain joints should be considered
+# at positions other than the current ones, these positions should
+# be set here
+motion_planning_msgs/KinematicJoint[] start_state
+
+
+# The joint names, in the same order as the values in the state
+string[] names
+
+# A list of states, each with the dimension of the requested model.
+# The dimension of the model may be different from the number of joints
+# (when there are joints that use more than one parameter)
+# Each state is to be used as a hint (initialization) for the search
+# if the valid state
+motion_planning_msgs/KinematicState[] states
+
+# The input constraints, to be converted to a set of joint constraints
+motion_planning_msgs/KinematicConstraints constraints
+
+# The maximum amount of time the search is to be run for
+float64 allowed_time
+
+---
+
+# The set of joint constraints that correspond to the pose constraint
+motion_planning_msgs/JointConstraint[] joint_constraint
Property changes on: pkg/trunk/motion_planning/motion_planning_msgs/srv/ConvertToJointConstraint.srv
___________________________________________________________________
Added: svn:mergeinfo
+ /pkg/branches/gazebo-branch-merge/motion_planning/motion_planning_msgs/srv/ConvertPoseToJointConstraint.srv:15683-15684,15739-15794,15797-15820,15822-15839,15852-15870,15983-16008,16010-16016,16129-16141,16145-16169,16245-16262,16274-16334
Modified: pkg/trunk/motion_planning/ompl_planning/src/motion_planner.cpp
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/src/motion_planner.cpp 2009-08-02 22:57:21 UTC (rev 20450)
+++ pkg/trunk/motion_planning/ompl_planning/src/motion_planner.cpp 2009-08-02 23:05:07 UTC (rev 20451)
@@ -36,7 +36,6 @@
#include "ompl_planning/Model.h"
#include "request_handler/RequestHandler.h"
-#include <motion_planning_msgs/ConvertPoseToJointConstraint.h>
using namespace ompl_planning;
@@ -180,7 +179,7 @@
return st;
}
- bool findValidState(motion_planning_msgs::ConvertPoseToJointConstraint::Request &req, motion_planning_msgs::ConvertPoseToJointConstraint::Response &res)
+ bool findValidState(motion_planning_msgs::ConvertToJointConstraint::Request &req, motion_planning_msgs::ConvertToJointConstraint::Response &res)
{
ROS_INFO("Received request for searching a valid state");
bool st = false;
Modified: pkg/trunk/motion_planning/ompl_planning/src/request_handler/RequestHandler.cpp
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/src/request_handler/RequestHandler.cpp 2009-08-02 22:57:21 UTC (rev 20450)
+++ pkg/trunk/motion_planning/ompl_planning/src/request_handler/RequestHandler.cpp 2009-08-02 23:05:07 UTC (rev 20451)
@@ -40,7 +40,7 @@
#include <sstream>
#include <cstdlib>
-bool ompl_planning::RequestHandler::isRequestValid(ModelMap &models, motion_planning_msgs::ConvertPoseToJointConstraint::Request &req)
+bool ompl_planning::RequestHandler::isRequestValid(ModelMap &models, motion_planning_msgs::ConvertToJointConstraint::Request &req)
{
ModelMap::const_iterator pos = models.find(req.params.model_id);
@@ -186,7 +186,7 @@
return true;
}
-void ompl_planning::RequestHandler::configure(const planning_models::StateParams *startState, motion_planning_msgs::ConvertPoseToJointConstraint::Request &req, IKSetup *iksetup)
+void ompl_planning::RequestHandler::configure(const planning_models::StateParams *startState, motion_planning_msgs::ConvertToJointConstraint::Request &req, IKSetup *iksetup)
{
/* clear memory */
iksetup->si->clearGoal();
@@ -332,8 +332,8 @@
return result;
}
-bool ompl_planning::RequestHandler::findState(ModelMap &models, const planning_models::StateParams *start, motion_planning_msgs::ConvertPoseToJointConstraint::Request &req,
- motion_planning_msgs::ConvertPoseToJointConstraint::Response &res)
+bool ompl_planning::RequestHandler::findState(ModelMap &models, const planning_models::StateParams *start, motion_planning_msgs::ConvertToJointConstraint::Request &req,
+ motion_planning_msgs::ConvertToJointConstraint::Response &res)
{
if (!isRequestValid(models, req))
return false;
Modified: pkg/trunk/motion_planning/ompl_planning/src/request_handler/RequestHandler.h
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/src/request_handler/RequestHandler.h 2009-08-02 22:57:21 UTC (rev 20450)
+++ pkg/trunk/motion_planning/ompl_planning/src/request_handler/RequestHandler.h 2009-08-02 23:05:07 UTC (rev 20451)
@@ -40,7 +40,7 @@
#include "ompl_planning/Model.h"
#include <ros/ros.h>
#include <motion_planning_msgs/GetMotionPlan.h>
-#include <motion_planning_msgs/ConvertPoseToJointConstraint.h>
+#include <motion_planning_msgs/ConvertToJointConstraint.h>
/** \brief Main namespace */
namespace ompl_planning
@@ -85,13 +85,13 @@
bool isRequestValid(ModelMap &models, motion_planning_msgs::GetMotionPlan::Request &req);
/** \brief Check if the request is valid */
- bool isRequestValid(ModelMap &models, motion_planning_msgs::ConvertPoseToJointConstraint::Request &req);
+ bool isRequestValid(ModelMap &models, motion_planning_msgs::ConvertToJointConstraint::Request &req);
/** \brief Check and compute a motion plan. Return true if the plan was succesfully computed */
bool computePlan(ModelMap &models, const planning_models::StateParams *start, motion_planning_msgs::GetMotionPlan::Request &req, motion_planning_msgs::GetMotionPlan::Response &res);
/** \brief Find a state in the specified goal region. Return true if state was found */
- bool findState(ModelMap &models, const planning_models::StateParams *start, motion_planning_msgs::ConvertPoseToJointConstraint::Request &req, motion_planning_msgs::ConvertPoseToJointConstraint::Response &res);
+ bool findState(ModelMap &models, const planning_models::StateParams *start, motion_planning_msgs::ConvertToJointConstraint::Request &req, motion_planning_msgs::ConvertToJointConstraint::Response &res);
private:
@@ -106,7 +106,7 @@
void configure(const planning_models::StateParams *startState, motion_planning_msgs::GetMotionPlan::Request &req, PlannerSetup *psetup);
/** \brief Set up all the data needed by inverse kinematics based on a request */
- void configure(const planning_models::StateParams *startState, motion_planning_msgs::ConvertPoseToJointConstraint::Request &req, IKSetup *iksetup);
+ void configure(const planning_models::StateParams *startState, motion_planning_msgs::ConvertToJointConstraint::Request &req, IKSetup *iksetup);
/** \brief Compute the actual motion plan. Return true if computed plan was trivial (start state already in goal region) */
bool callPlanner(PlannerSetup *psetup, int times, double allowed_time, Solution &sol);
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <is...@us...> - 2009-08-03 02:22:36
|
Revision: 20458
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=20458&view=rev
Author: isucan
Date: 2009-08-03 02:03:47 +0000 (Mon, 03 Aug 2009)
Log Message:
-----------
small patches
Modified Paths:
--------------
pkg/trunk/demos/tabletop_manipulation/tabletop_scripts/manifest.xml
pkg/trunk/demos/tabletop_manipulation/tabletop_scripts/src/simple.cpp
pkg/trunk/motion_planning/planning_environment/src/monitors/collision_space_monitor.cpp
pkg/trunk/sandbox/3dnav_pr2/launch/planning/ompl_planning.launch
pkg/trunk/sandbox/3dnav_pr2/launch/pr2_planning_environment.launch
Modified: pkg/trunk/demos/tabletop_manipulation/tabletop_scripts/manifest.xml
===================================================================
--- pkg/trunk/demos/tabletop_manipulation/tabletop_scripts/manifest.xml 2009-08-03 00:25:17 UTC (rev 20457)
+++ pkg/trunk/demos/tabletop_manipulation/tabletop_scripts/manifest.xml 2009-08-03 02:03:47 UTC (rev 20458)
@@ -6,6 +6,7 @@
<url>http://pr.willowgarage.com/wiki</url>
<depend package="3dnav_pr2"/>
+ <depend package="planning_environment"/>
<!--
<depend package="2dnav_pr2"/>
Modified: pkg/trunk/demos/tabletop_manipulation/tabletop_scripts/src/simple.cpp
===================================================================
--- pkg/trunk/demos/tabletop_manipulation/tabletop_scripts/src/simple.cpp 2009-08-03 00:25:17 UTC (rev 20457)
+++ pkg/trunk/demos/tabletop_manipulation/tabletop_scripts/src/simple.cpp 2009-08-03 02:03:47 UTC (rev 20458)
@@ -40,6 +40,7 @@
#include <robot_actions/action_client.h>
#include <mapping_msgs/ObjectInMap.h>
#include <mapping_msgs/AttachedObject.h>
+#include <pr2_mechanism_controllers/TrajectoryStart.h>
#include <pr2_robot_actions/MoveArmGoal.h>
#include <pr2_robot_actions/MoveArmState.h>
@@ -50,6 +51,7 @@
#include <recognition_lambertian/FindObjectPoses.h>
#include <visualization_msgs/Marker.h>
+
class CleanTable
{
@@ -58,7 +60,7 @@
CleanTable(void) : move_arm("move_right_arm"), gripper("actuate_gripper_right_arm")
{
pubObj = nh.advertise<mapping_msgs::ObjectInMap>("object_in_map", 1);
- pubAttach = nh.advertise<mapping_msgs::AttachedObject>("attach_map", 1);
+ pubAttach = nh.advertise<mapping_msgs::AttachedObject>("attach_object", 1);
vmPub = nh.advertise<visualization_msgs::Marker>("visualization_marker", 10240);
}
@@ -109,7 +111,7 @@
// hack for stereo error
double dx = 0.055;
double dy = 0.02;
- double dz = -0.04;
+ double dz = -0.0;
obj.object_pose.pose.position.x += dx;
obj.object_pose.pose.position.y += dy;
@@ -132,6 +134,14 @@
ROS_INFO("pose of object: %f %f %f", obj.object_pose.pose.position.x, obj.object_pose.pose.position.y, obj.object_pose.pose.position.z);
ROS_INFO("point to grasp: %f %f %f", obj.grasp_pose.pose.position.x, obj.grasp_pose.pose.position.y, obj.grasp_pose.pose.position.z);
+
+ mapping_msgs::ObjectInMap o;
+ o.header = obj.object_pose.header;
+ o.id = "ID";
+ o.action = mapping_msgs::ObjectInMap::ADD;
+ o.object = obj.object;
+ o.pose = obj.object_pose.pose;
+ pubObj.publish(o);
return true;
}
@@ -154,11 +164,11 @@
goal.goal_constraints.pose_constraint[0].position_tolerance_above.x = 0.005;
goal.goal_constraints.pose_constraint[0].position_tolerance_below.x = 0.005;
- goal.goal_constraints.pose_constraint[0].position_tolerance_above.y = 0.01;
- goal.goal_constraints.pose_constraint[0].position_tolerance_below.y = 0.01;
+ goal.goal_constraints.pose_constraint[0].position_tolerance_above.y = 0.02;
+ goal.goal_constraints.pose_constraint[0].position_tolerance_below.y = 0.02;
- goal.goal_constraints.pose_constraint[0].position_tolerance_above.z = 0.02;
- goal.goal_constraints.pose_constraint[0].position_tolerance_below.z = 0.02;
+ goal.goal_constraints.pose_constraint[0].position_tolerance_above.z = 0.03;
+ goal.goal_constraints.pose_constraint[0].position_tolerance_below.z = 0.03;
goal.goal_constraints.pose_constraint[0].orientation_tolerance_above.x = 0.1;
goal.goal_constraints.pose_constraint[0].orientation_tolerance_below.x = 0.1;
@@ -187,7 +197,7 @@
{
std_msgs::Float64 g, fb;
g.data = -80;
- if (gripper.execute(g, fb, ros::Duration(5.0)) != robot_actions::SUCCESS)
+ if (gripper.execute(g, fb, ros::Duration(15.0)) != robot_actions::SUCCESS)
return true;
else
{
@@ -200,7 +210,7 @@
{
std_msgs::Float64 g, fb;
g.data = 80;
- if (gripper.execute(g, fb, ros::Duration(5.0)) != robot_actions::SUCCESS)
+ if (gripper.execute(g, fb, ros::Duration(15.0)) != robot_actions::SUCCESS)
return true;
else
{
@@ -213,12 +223,20 @@
{
if (gripClose())
{
+ mapping_msgs::ObjectInMap o;
+ o.header.frame_id = obj.object_pose.header.frame_id;
+ o.header.stamp = ros::Time::now();
+ o.id = "ID";
+ o.action = mapping_msgs::ObjectInMap::REMOVE;
+ pubObj.publish(o);
+
mapping_msgs::AttachedObject ao;
ao.header = obj.object_pose.header;
ao.link_name = "r_wrist_roll_link";
ao.objects.push_back(obj.object);
ao.poses.push_back(obj.object_pose.pose);
pubAttach.publish(ao);
+
return true;
}
else return false;
@@ -244,6 +262,7 @@
robot_actions::ActionClient<std_msgs::Float64, pr2_robot_actions::ActuateGripperState, std_msgs::Float64> gripper;
ros::NodeHandle nh;
+
ros::Publisher pubObj;
ros::Publisher pubAttach;
ros::Publisher vmPub;
Modified: pkg/trunk/motion_planning/planning_environment/src/monitors/collision_space_monitor.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_environment/src/monitors/collision_space_monitor.cpp 2009-08-03 00:25:17 UTC (rev 20457)
+++ pkg/trunk/motion_planning/planning_environment/src/monitors/collision_space_monitor.cpp 2009-08-03 02:03:47 UTC (rev 20458)
@@ -64,7 +64,7 @@
haveMap_ = false;
collisionSpace_ = cm_->getODECollisionModel().get();
- nh_.param<double>("~pointcloud_padd", pointcloud_padd_, 0.01);
+ nh_.param<double>("~pointcloud_padd", pointcloud_padd_, 0.02);
startEnvironmentMonitor();
}
Modified: pkg/trunk/sandbox/3dnav_pr2/launch/planning/ompl_planning.launch
===================================================================
--- pkg/trunk/sandbox/3dnav_pr2/launch/planning/ompl_planning.launch 2009-08-03 00:25:17 UTC (rev 20457)
+++ pkg/trunk/sandbox/3dnav_pr2/launch/planning/ompl_planning.launch 2009-08-03 02:03:47 UTC (rev 20458)
@@ -2,7 +2,7 @@
<include file="$(find 3dnav_pr2)/launch/prX.machine" />
- <node machine="two" pkg="ompl_planning" type="motion_planner" respawn="true" output="screen">
+ <node machine="two" launch-prefix="gdb --args" pkg="ompl_planning" type="motion_planner" respawn="true" output="screen">
<remap from="collision_map" to="collision_map_occ" />
<remap from="collision_map_update" to="collision_map_occ_update" />
Modified: pkg/trunk/sandbox/3dnav_pr2/launch/pr2_planning_environment.launch
===================================================================
--- pkg/trunk/sandbox/3dnav_pr2/launch/pr2_planning_environment.launch 2009-08-03 00:25:17 UTC (rev 20457)
+++ pkg/trunk/sandbox/3dnav_pr2/launch/pr2_planning_environment.launch 2009-08-03 02:03:47 UTC (rev 20458)
@@ -1,7 +1,7 @@
<launch>
<!-- send parameters for collision checking for PR2; this includes parameters for the self filter -->
- <rosparam command="load" ns="robot_description_collision" file="$(find 3dnav_pr2)/params/collision/collision_checks_both_arms.yaml" />
+ <rosparam command="load" ns="robot_description_collision" file="$(find 3dnav_pr2)/params/collision/collision_checks_right_arm.yaml" />
<!-- send parameters needed for motion planning -->
<rosparam command="load" ns="robot_description_planning" file="$(find 3dnav_pr2)/params/planning/planning.yaml" />
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <bla...@us...> - 2009-08-03 08:13:38
|
Revision: 20478
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=20478&view=rev
Author: blaisegassend
Date: 2009-08-03 08:13:29 +0000 (Mon, 03 Aug 2009)
Log Message:
-----------
Changed nomenclature in driver_base. Renamed method names to use camelCase.
Got forearm_node working with driver_base: now appears to be working well.
Modified Paths:
--------------
pkg/trunk/drivers/cam/forearm_cam/src/nodes/CMakeLists.txt
pkg/trunk/drivers/cam/forearm_cam/src/nodes/forearm_node.cpp
pkg/trunk/drivers/cam/forearm_cam/src/nodes/forearm_node_new.cpp
pkg/trunk/drivers/generic/dynamic_reconfigure/include/dynamic_reconfigure/reconfigurator.h
pkg/trunk/drivers/generic/dynamic_reconfigure/templates/ConfigReconfigurator.h
pkg/trunk/stacks/hardware_test/self_test/include/self_test/self_test.h
Added Paths:
-----------
pkg/trunk/stacks/driver_common/driver_base/include/driver_base/driver.h
pkg/trunk/stacks/driver_common/driver_base/include/driver_base/driver_node.h
Removed Paths:
-------------
pkg/trunk/drivers/cam/forearm_cam/src/fcamlib/pr2lib.c
pkg/trunk/stacks/driver_common/driver_base/include/driver_base/device.h
pkg/trunk/stacks/driver_common/driver_base/include/driver_base/driver.h
Deleted: pkg/trunk/drivers/cam/forearm_cam/src/fcamlib/pr2lib.c
===================================================================
--- pkg/trunk/drivers/cam/forearm_cam/src/fcamlib/pr2lib.c 2009-08-03 08:03:45 UTC (rev 20477)
+++ pkg/trunk/drivers/cam/forearm_cam/src/fcamlib/pr2lib.c 2009-08-03 08:13:29 UTC (rev 20478)
@@ -1,1289 +0,0 @@
-#include "pr2lib.h"
-#include <stdio.h>
-#include <stdlib.h>
-#include <string.h>
-#include <sys/time.h>
-#include <errno.h>
-#include <unistd.h>
-#include <stdbool.h>
-#include <net/if.h>
-
-#include "host_netutil.h"
-#include "ipcam_packet.h"
-
-/// Amount of time in microseconds that the host should wait for packet replies
-#define STD_REPLY_TIMEOUT SEC_TO_USEC(0.2)
-
-#define VMODEDEF(width, height, fps, hblank, vblank) { #width"x"#height"x"#fps, width, height, fps, hblank, vblank }
-const struct MT9VMode MT9VModes[MT9V_NUM_MODES] = {
- VMODEDEF(752,480,15, 974, 138),
- VMODEDEF(752,480,12.5, 848, 320),
- VMODEDEF(640,480,30, 372, 47),
- VMODEDEF(640,480,25, 543, 61),
- VMODEDEF(640,480,15, 873, 225),
- VMODEDEF(640,480,12.5, 960, 320),
- VMODEDEF(320,240,60, 106, 73),
- VMODEDEF(320,240,50, 180, 80),
- VMODEDEF(320,240,30, 332, 169),
- VMODEDEF(320,240,25, 180, 400)
-};
-
-/**
- * Returns the version information for the library
- *
- *
- * @return Returns the the library as an integer 0x0000MMNN, where MM = Major version and NN = Minor version
- */
-int pr2LibVersion() {
- return PR2LIB_VERSION;
-}
-
-
-/**
- * Waits for a PR2 StatusPacket on the specified socket for a specified duration.
- *
- * The Status type and code will be reported back to the called via the 'type' & 'code'
- * arguments. If the timeout expires before a valid status packet is received, then
- * the function will still return zero but will indicate that a TIMEOUT error has occurred.
- *
- * @param s The open, bound & 'connected' socket to listen on
- * @param wait_us The number of microseconds to wait before timing out
- * @param type Points to the location to store the type of status packet
- * @param code Points to the location to store the subtype/error code
- *
- * @return Returns 0 if no system errors occured. -1 with errno set otherwise.
- */
-int pr2StatusWait( int s, uint32_t wait_us, uint32_t *type, uint32_t *code ) {
- if( !wgWaitForPacket(s, PKTT_STATUS, sizeof(PacketStatus), &wait_us) && (wait_us != 0) ) {
- PacketStatus sPkt;
- if( recvfrom( s, &sPkt, sizeof(PacketStatus), 0, NULL, NULL ) == -1 ) {
- perror("wgDiscover unable to receive from socket");
- *type = PKT_STATUST_ERROR;
- *code = PKT_ERROR_SYSERR;
- return -1;
- }
-
- *type = ntohl(sPkt.status_type);
- *code = ntohl(sPkt.status_code);
- return 0;
- }
-
- *type = PKT_STATUST_ERROR;
- *code = PKT_ERROR_TIMEOUT;
- return 0;
-}
-
-
-/**
- * Discovers all PR2 cameras that are connected to the 'ifName' ethernet interface and
- * adds new ones to the 'ipCamList' list.
- *
- * @param ifName The ethernet interface name to use. Null terminated string (e.g., "eth0").
- * @param ipCamList The list to which the new cameras should be added
- * @param wait_us The number of microseconds to wait for replies
- *
- * @return Returns -1 with errno set for system call errors. Otherwise returns number of new
- * cameras found.
- */
-int pr2Discover(const char *ifName, IpCamList *ipCamList, const char *ipAddress, unsigned wait_us) {
- // Create and initialize a new Discover packet
- PacketDiscover dPkt;
- dPkt.hdr.magic_no = htonl(WG_MAGIC_NO);
- dPkt.hdr.type = htonl(PKTT_DISCOVER);
- strncpy(dPkt.hdr.hrt, "DISCOVER", sizeof(dPkt.hdr.hrt));
-
- /* Create a new socket bound to a local ephemeral port, and get our local connection
- * info so we can request a reply back to the same socket.
- */
- int s = wgCmdSocketCreate(ifName, &dPkt.hdr.reply_to);
- if(s == -1) {
- perror("Unable to create socket\n");
- return -1;
- }
-
- //// Determine the broadcast address for ifName. This is the address we
- //// will tell the camera to send from.
-
- struct in_addr newIP;
- if (ipAddress) // An IP was specified
- {
- inet_aton(ipAddress, &newIP);
- dPkt.ip_addr = newIP.s_addr;
- }
- else // We guess an IP by flipping the host bits of the local IP.
- {
- struct in_addr localip;
- struct in_addr netmask;
- wgIpGetLocalAddr(ifName, &localip);
- wgIpGetLocalNetmask(ifName, &netmask);
- dPkt.ip_addr = localip.s_addr ^ netmask.s_addr ^ ~0;
- }
-
- if( wgSendUDPBcast(s, ifName, &dPkt,sizeof(dPkt)) == -1) {
- perror("Unable to send broadcast\n");
- }
-
- // For the old API
- if( wgSendUDPBcast(s, ifName, &dPkt,sizeof(dPkt)-sizeof(dPkt.ip_addr)) == -1) {
- perror("Unable to send broadcast\n");
- }
-
- // Count the number of new cameras found
- int newCamCount = 0;
- do {
- // Wait in the loop for replies. wait_us is updated each time through the loop.
- if( !wgWaitForPacket(s, PKTT_ANNOUNCE, sizeof(PacketAnnounce), &wait_us) && (wait_us != 0) ) {
- // We've received an Announce packet, so pull it out of the receive queue
- PacketAnnounce aPkt;
- struct sockaddr_in fromaddr;
- fromaddr.sin_family = AF_INET;
- socklen_t fromlen = sizeof(fromaddr);
-
- if( recvfrom( s, &aPkt, sizeof(PacketAnnounce), 0, (struct sockaddr *) &fromaddr, &fromlen) == -1 ) {
- perror("wgDiscover unable to receive from socket");
- close(s);
- return -1;
- }
-
- // Create a new list entry and initialize it
- IpCamList *tmpListItem;
- if( (tmpListItem = (IpCamList *)malloc(sizeof(IpCamList))) == NULL ) {
- perror("Malloc failed");
- close(s);
- return -1;
- }
- pr2CamListInit( tmpListItem );
-
- // Initialize the new list item's data fields (byte order corrected)
- tmpListItem->hw_version = ntohl(aPkt.hw_version);
- tmpListItem->fw_version = ntohl(aPkt.fw_version);
- tmpListItem->ip = fromaddr.sin_addr.s_addr;
- tmpListItem->serial = ntohl(aPkt.ser_no);
- memcpy(&tmpListItem->mac, aPkt.mac, sizeof(aPkt.mac));
- strncpy(tmpListItem->ifName, ifName, sizeof(tmpListItem->ifName));
- tmpListItem->status = CamStatusDiscovered;
- char pcb_rev = 0x0A + (0x0000000F & ntohl(aPkt.hw_version));
- int hdl_rev = 0x00000FFF & (ntohl(aPkt.hw_version)>>4);
- snprintf(tmpListItem->hwinfo, PR2_CAMINFO_LEN, "PCB rev %X : HDL rev %3X : FW rev %3X", pcb_rev, hdl_rev, ntohl(aPkt.fw_version));
-
- // If this camera is already in the list, we don't want to add another copy
- if( pr2CamListAdd( ipCamList, tmpListItem ) == CAMLIST_ADD_DUP) {
- free(tmpListItem);
- } else {
- debug("MAC Address: %02X:%02X:%02X:%02X:%02X:%02X\n", aPkt.mac[0], aPkt.mac[1], aPkt.mac[2], aPkt.mac[3], aPkt.mac[4], aPkt.mac[5]);
- debug("Product #%07u : Unit #%04u\n", ntohl(aPkt.product_id), ntohl(aPkt.ser_no));
- debug("%s\n", tmpListItem->hwinfo);
- newCamCount++;
- }
- }
- } while(wait_us > 0);
-
- close(s);
- return newCamCount;
-}
-
-
-/**
- * Configures the IP address of one specific camera.
- *
- * @param camInfo Structure describing the camera to configure
- * @param ipAddress An ASCII string containing the new IP address to assign (e.g., "192.168.0.5")
- * @param wait_us The number of microseconds to wait for a reply from the camera
- *
- * @return Returns -1 with errno set for system call failures.
- * Returns 0 for success
- * Returns ERR_TIMEOUT if no reply is received before the timeout expires
- * Returns ERR_CONFIG_ARPFAIL if the camera was configured successfully but
- * the host system could not update its arp table. This is normally because
- * the library is not running as root.
- */
-int pr2Configure( IpCamList *camInfo, const char *ipAddress, unsigned wait_us) {
- // Create and initialize a new Configure packet
- PacketConfigure cPkt;
-
- cPkt.hdr.magic_no = htonl(WG_MAGIC_NO);
- cPkt.hdr.type = htonl(PKTT_CONFIGURE);
- cPkt.product_id = htonl(CONFIG_PRODUCT_ID);
- strncpy(cPkt.hdr.hrt, "CONFIGURE", sizeof(cPkt.hdr.hrt));
-
- cPkt.ser_no = htonl(camInfo->serial);
-
- struct in_addr newIP;
- inet_aton(ipAddress, &newIP);
- cPkt.ip_addr = newIP.s_addr;
-
-
- // Create and send the Configure broadcast packet. It is sent broadcast
- // because the camera does not yet have an IP address assigned.
-
- /* Create a new socket bound to a local ephemeral port, and get our local connection
- * info so we can request a reply back to the same socket.
- */
- int s = wgCmdSocketCreate(camInfo->ifName, &cPkt.hdr.reply_to);
- if(s == -1) {
- perror("pr2Configure socket creation failed");
- return -1;
- }
-
- if(wgSendUDPBcast(s, camInfo->ifName, &cPkt, sizeof(cPkt)) == -1) {
- debug("Unable to send broadcast\n");
- close(s);
- return -1;
- }
-
-
- // 'Connect' insures we will only receive datagram replies from the camera's new IP
- IPAddress camIP = newIP.s_addr;
- if( wgSocketConnect(s, &camIP) ) {
- close(s);
- return -1;
- }
-
- // Wait up to wait_us for a valid packet to be received on s
- do {
- if( !wgWaitForPacket(s, PKTT_ANNOUNCE, sizeof(PacketAnnounce), &wait_us) && (wait_us != 0) ) {
- PacketAnnounce aPkt;
-
- if( recvfrom( s, &aPkt, sizeof(PacketAnnounce), 0, NULL, NULL ) == -1 ) {
- perror("wgDiscover unable to receive from socket");
- close(s);
- return -1;
- }
-
- // Need to have a valid packet from a camera with the expected serial number
- if( ntohl(aPkt.ser_no) == camInfo->serial ) {
- camInfo->status = CamStatusConfigured;
- memcpy(&camInfo->ip, &cPkt.ip_addr, sizeof(IPAddress));
- // Add the IP/MAC mapping to the system ARP table
- if( wgArpAdd(camInfo) ) {
- close(s);
- return ERR_CONFIG_ARPFAIL;
- }
- break;
- }
- }
- } while(wait_us > 0);
- close(s);
-
- if(wait_us != 0) {
- return 0;
- } else {
- return ERR_TIMEOUT;
- }
-}
-
-/**
- * Instructs one camera to begin streaming video to the host/port specified.
- *
- * @param camInfo Structure that describes the camera to contact
- * @param mac Contains the MAC address of the host that will receive the video
- * @param ipAddress An ASCII string in dotted quad form containing the IP address of the host
- * that will receive the video (e.g., "192.168.0.5")
- * @param port The port number that the video should be sent to. Host byte order.
- *
- * @return Returns -1 with errno set for system call failures
- * Returns 0 for success
- * Returns 1 for protocol failures (timeout, etc)
- */
-int pr2StartVid( const IpCamList *camInfo, const uint8_t mac[6], const char *ipAddress, uint16_t port ) {
- PacketVidStart vPkt;
-
- // Initialize the packet
- vPkt.hdr.magic_no = htonl(WG_MAGIC_NO);
- vPkt.hdr.type = htonl(PKTT_VIDSTART);
- strncpy(vPkt.hdr.hrt, "Start Video", sizeof(vPkt.hdr.hrt));
-
-
- // Convert the ipAddress string into a binary value in network byte order
- inet_aton(ipAddress, (struct in_addr*)&vPkt.receiver.addr);
-
- // Convert the receiver port into network byte order
- vPkt.receiver.port = htons(port);
-
- // Copy the MAC address into the vPkt
- memcpy(&vPkt.receiver.mac, mac, 6);
-
- /* Create a new socket bound to a local ephemeral port, and get our local connection
- * info so we can request a reply back to the same socket.
- */
- int s = wgCmdSocketCreate(camInfo->ifName, &vPkt.hdr.reply_to);
- if( s == -1 ) {
- return -1;
- }
-
- if(wgSendUDP(s, &camInfo->ip, &vPkt, sizeof(vPkt)) == -1) {
- goto err_out;
- }
-
- // 'Connect' insures we will only receive datagram replies from the camera we've specified
- if( wgSocketConnect(s, &camInfo->ip) ) {
- goto err_out;
- }
-
- // Wait for a status reply
- uint32_t type, code;
- if( pr2StatusWait( s, STD_REPLY_TIMEOUT, &type, &code ) == -1) {
- goto err_out;
- }
-
- close(s);
- if(type == PKT_STATUST_OK) {
- return 0;
- } else {
- debug("Error: wgStatusWait returned status %d, code %d\n", type, code);
- return 1;
- }
-
-err_out:
- close(s);
- return -1;
-}
-
-/**
- * Instructs one camera to stop sending video.
- *
- * @param camInfo Describes the camera to send the stop to.
- * @return Returns 0 for success
- * Returns -1 with errno set for system errors
- * Returns 1 for protocol errors
- */
-int pr2StopVid( const IpCamList *camInfo ) {
- PacketVidStop vPkt;
-
- // Initialize the packet
- vPkt.hdr.magic_no = htonl(WG_MAGIC_NO);
- vPkt.hdr.type = htonl(PKTT_VIDSTOP);
- strncpy(vPkt.hdr.hrt, "Stop Video", sizeof(vPkt.hdr.hrt));
-
- /* Create a new socket bound to a local ephemeral port, and get our local connection
- * info so we can request a reply back to the same socket.
- */
- int s = wgCmdSocketCreate(camInfo->ifName, &vPkt.hdr.reply_to);
- if( s == -1 ) {
- return -1;
- }
-
- if( wgSendUDP(s, &camInfo->ip, &vPkt, sizeof(vPkt)) == -1 ) {
- goto err_out;
- }
-
- // 'Connect' insures we will only receive datagram replies from the camera we've specified
- if( wgSocketConnect(s, &camInfo->ip) == -1) {
- goto err_out;
- }
-
- uint32_t type, code;
- if(pr2StatusWait( s, STD_REPLY_TIMEOUT, &type, &code ) == -1) {
- goto err_out;
- }
-
- close(s);
- if(type == PKT_STATUST_OK) {
- return 0;
- } else {
- debug("Error: pr2StatusWait returned status %d, code %d\n", type, code);
- return 1;
- }
-
-err_out:
- close(s);
- return -1;
-}
-
-/**
- * Instructs one camera to execute a soft reset.
- *
- * @param camInfo Describes the camera to reset.
- * @return Returns 0 for success
- * Returns -1 for system errors
- */
-int pr2Reset( IpCamList *camInfo ) {
- PacketReset gPkt;
-
- // Initialize the packet
- gPkt.hdr.magic_no = htonl(WG_MAGIC_NO);
- gPkt.hdr.type = htonl(PKTT_RESET);
- strncpy(gPkt.hdr.hrt, "Reset", sizeof(gPkt.hdr.hrt));
-
- /* Create a new socket bound to a local ephemeral port, and get our local connection
- * info so we can request a reply back to the same socket.
- */
- int s = wgCmdSocketCreate(camInfo->ifName, &gPkt.hdr.reply_to);
- if( s == -1 ) {
- return -1;
- }
-
- if( wgSendUDP(s, &camInfo->ip, &gPkt, sizeof(gPkt)) == -1 ) {
- close(s);
- return -1;
- }
-
-
- close(s);
-
- // Camera is no longer configured after a reset
- camInfo->status = CamStatusDiscovered;
-
- // There is no reply to a reset packet
-
- return 0;
-}
-
-/**
- * Gets the system time of a specified camera.
- *
- * In the camera, system time is tracked as a number of clock 'ticks' since
- * the last hard reset. This function receives the number of 'ticks' as well as
- * a 'ticks_per_sec' conversion factor to return a 64-bit time result in microseconds.
- *
- * @param camInfo Describes the camera to connect to
- * @param time_us Points to the location to store the system time in us
- *
- * @return Returns 0 for success, -1 for system error, or 1 for protocol error.
- */
-int pr2GetTimer( const IpCamList *camInfo, uint64_t *time_us ) {
- PacketTimeRequest gPkt;
-
- // Initialize the packet
- gPkt.hdr.magic_no = htonl(WG_MAGIC_NO);
- gPkt.hdr.type = htonl(PKTT_TIMEREQ);
- strncpy(gPkt.hdr.hrt, "Time Req", sizeof(gPkt.hdr.hrt));
-
- /* Create a new socket bound to a local ephemeral port, and get our local connection
- * info so we can request a reply back to the same socket.
- */
- int s = wgCmdSocketCreate(camInfo->ifName, &gPkt.hdr.reply_to);
- if( s == -1 ) {
- return -1;
- }
-
- if( wgSendUDP(s, &camInfo->ip, &gPkt, sizeof(gPkt)) == -1 ) {
- close(s);
- return -1;
- }
-
- // 'Connect' insures we will only receive datagram replies from the camera we've specified
- if( wgSocketConnect(s, &camInfo->ip) ) {
- close(s);
- return -1;
- }
-
- uint32_t wait_us = STD_REPLY_TIMEOUT;
- do {
- if( !wgWaitForPacket(s, PKTT_TIMEREPLY, sizeof(PacketTimer), &wait_us) && (wait_us != 0) ) {
- PacketTimer tPkt;
- if( recvfrom( s, &tPkt, sizeof(PacketTimer), 0, NULL, NULL ) == -1 ) {
- perror("GetTime unable to receive from socket");
- close(s);
- return -1;
- }
-
- *time_us = (uint64_t)ntohl(tPkt.ticks_hi) << 32;
- *time_us += ntohl(tPkt.ticks_lo);
-
- // Need to multiply final result by 1E6 to get us from sec
- // Try to do this to limit loss of precision without overflow
- // (We will overflow the 64-bit type with this approach
- // after the camera has been up for about 11 continuous years)
- //FIXME: Review this algorithm for possible improvements.
- *time_us *= 1000;
- *time_us /= (ntohl(tPkt.ticks_per_sec)/1000);
- //debug("Got time of %llu us since last reset\n", *time_us);
- close(s);
- return 0;
- }
- } while(wait_us > 0);
-
- debug("Timed out waiting for time value\n");
- close(s);
- return 1;
-}
-
-/**
- * Reads one FLASH_PAGE_SIZE byte page of the camera's onboard Atmel dataflash.
- *
- * @param camInfo Describes the camera to connect to.
- * @param address Specifies the 12-bit flash page address to read (0-4095)
- * @param pageDataOut Points to at least FLASH_PAGE_SIZE bytes of storage in which to place the flash data.
- *
- * @return Returns 0 for success
- * Returns -1 for system error
- * Returns 1 for protocol error
- *
- */
-
-int pr2FlashRead( const IpCamList *camInfo, uint32_t address, uint8_t *pageDataOut ) {
- PacketFlashRequest rPkt;
-
- // Initialize the packet
- rPkt.hdr.magic_no = htonl(WG_MAGIC_NO);
- rPkt.hdr.type = htonl(PKTT_FLASHREAD);
- if(address > FLASH_MAX_PAGENO) {
- return 1;
- }
-
- // The page portion of the address is 12-bits wide, range (bit9..bit21)
- rPkt.address = htonl(address<<9);
-
- strncpy(rPkt.hdr.hrt, "Flash read", sizeof(rPkt.hdr.hrt));
-
- /* Create a new socket bound to a local ephemeral port, and get our local connection
- * info so we can request a reply back to the same socket.
- */
- int s = wgCmdSocketCreate(camInfo->ifName, &rPkt.hdr.reply_to);
- if( s == -1 ) {
- return -1;
- }
-
- if( wgSendUDP(s, &camInfo->ip, &rPkt, sizeof(rPkt)) == -1 ) {
- close(s);
- return -1;
- }
-
-
- // 'Connect' insures we will only receive datagram replies from the camera we've specified
- if( wgSocketConnect(s, &camInfo->ip) ) {
- close(s);
- return -1;
- }
-
- uint32_t wait_us = STD_REPLY_TIMEOUT;
- do {
- if( !wgWaitForPacket(s, PKTT_FLASHDATA, sizeof(PacketFlashPayload), &wait_us) && (wait_us != 0) ) {
- PacketFlashPayload fPkt;
- if( recvfrom( s, &fPkt, sizeof(PacketFlashPayload), 0, NULL, NULL ) == -1 ) {
- perror("GetTime unable to receive from socket");
- close(s);
- return -1;
- }
-
- // Copy the received data into the space pointed to by pageDataOut
- memcpy(pageDataOut, fPkt.data, FLASH_PAGE_SIZE);
- close(s);
- return 0;
- }
- } while(wait_us > 0);
-
- debug("Timed out waiting for flash value\n");
- close(s);
- return 1;
-}
-
-/**
- * Writes one FLASH_PAGE_SIZE byte page to the camera's onboard Atmel dataflash.
- *
- * @param camInfo Describes the camera to connect to.
- * @param address Specifies the 12-bit flash page address to write (0-4095)
- * @param pageDataOut Points to at least FLASH_PAGE_SIZE bytes of storage from which to get the flash data.
- *
- * @return Returns 0 for success
- * Returns -1 for system error
- * Returns 1 for protocol error
- *
- */
-int pr2FlashWrite( const IpCamList *camInfo, uint32_t address, const uint8_t *pageDataIn ) {
- PacketFlashPayload rPkt;
-
- // Initialize the packet
- rPkt.hdr.magic_no = htonl(WG_MAGIC_NO);
- rPkt.hdr.type = htonl(PKTT_FLASHWRITE);
- if(address > FLASH_MAX_PAGENO) {
- return 1;
- }
-
- // The page portion of the address is 12-bits wide, range (bit9..bit21)
- rPkt.address = htonl(address<<9);
- strncpy(rPkt.hdr.hrt, "Flash write", sizeof(rPkt.hdr.hrt));
-
- memcpy(rPkt.data, pageDataIn, FLASH_PAGE_SIZE);
-
- /* Create a new socket bound to a local ephemeral port, and get our local connection
- * info so we can request a reply back to the same socket.
- */
- int s = wgCmdSocketCreate(camInfo->ifName, &rPkt.hdr.reply_to);
- if( s == -1 ) {
- return -1;
- }
-
- if( wgSendUDP(s, &camInfo->ip, &rPkt, sizeof(rPkt)) == -1 ) {
- close(s);
- return -1;
- }
-
- // 'Connect' insures we will only receive datagram replies from the camera we've specified
- if( wgSocketConnect(s, &camInfo->ip) ) {
- close(s);
- return -1;
- }
-
- // Wait for response - once we get an OK, we're clear to send the next page.
- uint32_t type, code;
- pr2StatusWait( s, STD_REPLY_TIMEOUT, &type, &code );
-
- close(s);
- if(type == PKT_STATUST_OK) {
- return 0;
- } else {
- debug("Error: wgStatusWait returned status %d, code %d\n", type, code);
- return 1;
- }
-}
-
-/**
- * Sets the trigger type (internal or external) for one camera.
- *
- * @param camInfo Describes the camera to connect to
- * @param triggerType Can be either TRIG_STATE_INTERNAL or TRIG_STATE_EXTERNAL
- *
- * @return Returns 0 for success
- * Returns -1 for system error
- * Returns 1 for protocol error
- */
-int pr2TriggerControl( const IpCamList *camInfo, uint32_t triggerType ) {
- PacketTrigControl tPkt;
-
- // Initialize the packet
- tPkt.hdr.magic_no = htonl(WG_MAGIC_NO);
- tPkt.hdr.type = htonl(PKTT_TRIGCTRL);
- tPkt.trig_state = htonl(triggerType);
-
- if(triggerType == TRIG_STATE_INTERNAL ) {
- strncpy(tPkt.hdr.hrt, "Int. Trigger", sizeof(tPkt.hdr.hrt));
- } else {
- strncpy(tPkt.hdr.hrt, "Ext. Trigger", sizeof(tPkt.hdr.hrt));
- }
-
- /* Create a new socket bound to a local ephemeral port, and get our local connection
- * info so we can request a reply back to the same socket.
- */
- int s = wgCmdSocketCreate(camInfo->ifName, &tPkt.hdr.reply_to);
- if( s == -1 ) {
- return -1;
- }
-
- if( wgSendUDP(s, &camInfo->ip, &tPkt, sizeof(tPkt)) == -1 ) {
- close(s);
- return -1;
- }
-
- // 'Connect' insures we will only receive datagram replies from the camera we've specified
- if( wgSocketConnect(s, &camInfo->ip) ) {
- close(s);
- return -1;
- }
-
- // Wait for response
- uint32_t type, code;
- pr2StatusWait( s, STD_REPLY_TIMEOUT, &type, &code );
-
- close(s);
- if(type == PKT_STATUST_OK) {
- return 0;
- } else {
- debug("Error: wgStatusWait returned status %d, code %d\n", type, code);
- return 1;
- }
-}
-
-/**
- * Sets the permanent serial number and MAC configuration for one camera
- *
- * @warning: This command can only be sent once per camera. The changes are permanent.
- *
- * @param camInfo Describes the camera to connect to
- * @param serial Is the 32-bit unique serial number to assign (the product ID portion is already fixed)
- * @param mac Is the 48-bit unique MAC address to assign to the board
- *
- * @return Returns 0 for success
- * Returns -1 for system error
- * Returns 1 for protocol error
- */
-int pr2ConfigureBoard( const IpCamList *camInfo, uint32_t serial, MACAddress *mac ) {
- PacketSysConfig sPkt;
-
- // Initialize the packet
- sPkt.hdr.magic_no = htonl(WG_MAGIC_NO);
- sPkt.hdr.type = htonl(PKTT_SYSCONFIG);
-
- strncpy(sPkt.hdr.hrt, "System Config", sizeof(sPkt.hdr.hrt));
- memcpy(&sPkt.mac, mac, 6);
- sPkt.serial = htonl(serial);
-
-
- /* Create a new socket bound to a local ephemeral port, and get our local connection
- * info so we can request a reply back to the same socket.
- */
- int s = wgCmdSocketCreate(camInfo->ifName, &sPkt.hdr.reply_to);
- if( s == -1 ) {
- return -1;
- }
-
- if( wgSendUDP(s, &camInfo->ip, &sPkt, sizeof(sPkt)) == -1 ) {
- close(s);
- return -1;
- }
-
- // 'Connect' insures we will only receive datagram replies from the camera we've specified
- if( wgSocketConnect(s, &camInfo->ip) ) {
- close(s);
- return -1;
- }
- // Wait for response
- uint32_t type, code;
- pr2StatusWait( s, STD_REPLY_TIMEOUT, &type, &code );
-
- close(s);
- if(type == PKT_STATUST_OK) {
- return 0;
- } else {
- debug("Error: wgStatusWait returned status %d, code %d\n", type, code);
- return 1;
- }
-}
-
-/**
- * Writes to one image sensor I2C register on one camera.
- *
- * @param camInfo Describes the camera to connect to
- * @param reg The 8-bit register address to write into
- * @parm data 16-bit value to write into the register
- *
- * @return Returns 0 for success
- * Returns -1 for system error
- * Returns 1 for protocol error
- */
-int pr2SensorWrite( const IpCamList *camInfo, uint8_t reg, uint16_t data ) {
- PacketSensorData sPkt;
-
- // Initialize the packet
- sPkt.hdr.magic_no = htonl(WG_MAGIC_NO);
- sPkt.hdr.type = htonl(PKTT_SENSORWR);
-
- strncpy(sPkt.hdr.hrt, "Write I2C", sizeof(sPkt.hdr.hrt));
- sPkt.address = reg;
- sPkt.data = htons(data);
-
- /* Create a new socket bound to a local ephemeral port, and get our local connection
- * info so we can request a reply back to the same socket.
- */
- int s = wgCmdSocketCreate(camInfo->ifName, &sPkt.hdr.reply_to);
- if( s == -1 ) {
- return -1;
- }
-
- if( wgSendUDP(s, &camInfo->ip, &sPkt, sizeof(sPkt)) == -1 ) {
- close(s);
- return -1;
- }
-
- // 'Connect' insures we will only receive datagram replies from the camera we've specified
- if( wgSocketConnect(s, &camInfo->ip) ) {
- close(s);
- return -1;
- }
-
- // Wait for response
- uint32_t type, code;
- pr2StatusWait( s, STD_REPLY_TIMEOUT, &type, &code );
-
- close(s);
- if(type == PKT_STATUST_OK) {
- return 0;
- } else {
- debug("Error: wgStatusWait returned status %d, code %d\n", type, code);
- return 1;
- }
-}
-
-/**
- * Reads the value of one image sensor I2C register on one camera.
- *
- * @param camInfo Describes the camera to connect to
- * @param reg The 8-bit register address to read from
- * @parm data Pointer to 16 bits of storage to write the value to
- *
- * @return Returns 0 for success
- * Returns -1 for system error
- * Returns 1 for protocol error
- */
-int pr2SensorRead( const IpCamList *camInfo, uint8_t reg, uint16_t *data ) {
- PacketSensorRequest rPkt;
-
- // Initialize the packet
- rPkt.hdr.magic_no = htonl(WG_MAGIC_NO);
- rPkt.hdr.type = htonl(PKTT_SENSORRD);
- rPkt.address = reg;
- strncpy(rPkt.hdr.hrt, "Read I2C", sizeof(rPkt.hdr.hrt));
-
- /* Create a new socket bound to a local ephemeral port, and get our local connection
- * info so we can request a reply back to the same socket.
- */
- int s = wgCmdSocketCreate(camInfo->ifName, &rPkt.hdr.reply_to);
- if( s == -1 ) {
- return -1;
- }
-
- if( wgSendUDP(s, &camInfo->ip, &rPkt, sizeof(rPkt)) == -1 ) {
- close(s);
- return -1;
- }
-
- // 'Connect' insures we will only receive datagram replies from the camera we've specified
- if( wgSocketConnect(s, &camInfo->ip) ) {
- close(s);
- return -1;
- }
-
- uint32_t wait_us = STD_REPLY_TIMEOUT;
- do {
- if( !wgWaitForPacket(s, PKTT_SENSORDATA, sizeof(PacketSensorData), &wait_us) && (wait_us != 0) ) {
- PacketSensorData sPkt;
- if( recvfrom( s, &sPkt, sizeof(PacketSensorData), 0, NULL, NULL ) == -1 ) {
- perror("SensorRead unable to receive from socket");
- close(s);
- return -1;
- }
-
- *data = ntohs(sPkt.data);
- close(s);
- return 0;
- }
- } while(wait_us > 0);
-
- debug("Timed out waiting for sensor value\n");
- close(s);
- return 1;
-}
-
-/**
- * Sets the address of one of the four automatically read I2C registers on one camera.
- *
- * @param camInfo Describes the camera to connect to
- * @param index The index (0..3) of the register to set
- * @param reg The 8-bit address of the register to read at position 'index', or I2C_AUTO_REG_UNUSED.
- *
- * @return Returns 0 for success
- * Returns -1 for system error
- * Returns 1 for protocol error
- */
-int pr2SensorSelect( const IpCamList *camInfo, uint8_t index, uint32_t reg ) {
- PacketSensorSelect sPkt;
-
- // Initialize the packet
- sPkt.hdr.magic_no = htonl(WG_MAGIC_NO);
- sPkt.hdr.type = htonl(PKTT_SENSORSEL);
-
- strncpy(sPkt.hdr.hrt, "Select I2C", sizeof(sPkt.hdr.hrt));
- sPkt.index = index;
- sPkt.address = htonl(reg);
-
- /* Create a new socket bound to a local ephemeral port, and get our local connection
- * info so we can request a reply back to the same socket.
- */
- int s = wgCmdSocketCreate(camInfo->ifName, &sPkt.hdr.reply_to);
- if( s == -1 ) {
- return -1;
- }
-
- if( wgSendUDP(s, &camInfo->ip, &sPkt, sizeof(sPkt)) == -1 ) {
- close(s);
- return -1;
- }
-
- // 'Connect' insures we will only receive datagram replies from the camera we've specified
- if( wgSocketConnect(s, &camInfo->ip) ) {
- close(s);
- return -1;
- }
-
- // Wait for response
- uint32_t type, code;
- pr2StatusWait( s, STD_REPLY_TIMEOUT, &type, &code );
-
- close(s);
- if(type == PKT_STATUST_OK) {
- return 0;
- } else {
- debug("Error: wgStatusWait returned status %d, code %d\n", type, code);
- return 1;
- }
-}
-
-/**
- * Selects one of the 10 pre-programmed imager modes in the specified camera.
- *
- * @param camInfo Describes the camera to connect to
- * @param mode The mode number to select, range (0..9)
- *
- * @return Returns 0 for success
- * Returns -1 for system error
- * Returns 1 for protocol error
- */
-int pr2ImagerModeSelect( const IpCamList *camInfo, uint32_t mode ) {
- PacketImagerMode mPkt;
-
- // Initialize the packet
- mPkt.hdr.magic_no = htonl(WG_MAGIC_NO);
- mPkt.hdr.type = htonl(PKTT_IMGRMODE);
-
- mPkt.mode = htonl(mode);
-
- strncpy(mPkt.hdr.hrt, "Set Mode", sizeof(mPkt.hdr.hrt));
-
- /* Create a new socket bound to a local ephemeral port, and get our local connection
- * info so we can request a reply back to the same socket.
- */
- int s ...
[truncated message content] |
|
From: <mee...@us...> - 2009-08-03 20:13:47
|
Revision: 20519
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=20519&view=rev
Author: meeussen
Date: 2009-08-03 20:13:35 +0000 (Mon, 03 Aug 2009)
Log Message:
-----------
kdl segment/joint naming patch was applied upstream, so remove it locally. Fix two controllers that were missing includes and wrong jacobian construction
Modified Paths:
--------------
pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/endeffector_constraint_controller.h
pkg/trunk/controllers/robot_mechanism_controllers/src/endeffector_constraint_controller.cpp
pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/plug_controller.h
pkg/trunk/sandbox/experimental_controllers/src/plug_controller.cpp
pkg/trunk/stacks/geometry/kdl/Makefile
pkg/trunk/stacks/geometry/kdl/total.patch
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-08-03 20:07:05 UTC (rev 20518)
+++ pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/endeffector_constraint_controller.h 2009-08-03 20:13:35 UTC (rev 20519)
@@ -40,6 +40,7 @@
#include "kdl/chain.hpp"
#include "kdl/frames.hpp"
#include "kdl/chainfksolver.hpp"
+#include "kdl/chainjnttojacsolver.hpp"
#include "ros/node.h"
#include "robot_msgs/Wrench.h"
#include "misc_utils/subscription_guard.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-08-03 20:07:05 UTC (rev 20518)
+++ pkg/trunk/controllers/robot_mechanism_controllers/src/endeffector_constraint_controller.cpp 2009-08-03 20:13:35 UTC (rev 20519)
@@ -150,7 +150,7 @@
initialized_ = true;
}
- Jacobian jacobian(kdl_chain_.getNrOfJoints(), kdl_chain_.getNrOfSegments());
+ Jacobian jacobian(kdl_chain_.getNrOfJoints());
jnt_to_jac_solver_->JntToJac(jnt_pos, jacobian);
Modified: pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/plug_controller.h
===================================================================
--- pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/plug_controller.h 2009-08-03 20:07:05 UTC (rev 20518)
+++ pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/plug_controller.h 2009-08-03 20:13:35 UTC (rev 20519)
@@ -40,6 +40,7 @@
#include "kdl/chain.hpp"
#include "kdl/frames.hpp"
#include "kdl/chainfksolver.hpp"
+#include "kdl/chainjnttojacsolver.hpp"
#include "ros/node.h"
#include "robot_msgs/Wrench.h"
#include "robot_msgs/PoseStamped.h"
Modified: pkg/trunk/sandbox/experimental_controllers/src/plug_controller.cpp
===================================================================
--- pkg/trunk/sandbox/experimental_controllers/src/plug_controller.cpp 2009-08-03 20:07:05 UTC (rev 20518)
+++ pkg/trunk/sandbox/experimental_controllers/src/plug_controller.cpp 2009-08-03 20:13:35 UTC (rev 20519)
@@ -160,7 +160,7 @@
initialized_ = true;
}
- Jacobian jacobian(kdl_chain_.getNrOfJoints(), kdl_chain_.getNrOfSegments());
+ Jacobian jacobian(kdl_chain_.getNrOfJoints());
jnt_to_jac_solver_->JntToJac(jnt_pos, jacobian);
// TODO: Write a function for doing this conversion
Modified: pkg/trunk/stacks/geometry/kdl/Makefile
===================================================================
--- pkg/trunk/stacks/geometry/kdl/Makefile 2009-08-03 20:07:05 UTC (rev 20518)
+++ pkg/trunk/stacks/geometry/kdl/Makefile 2009-08-03 20:13:35 UTC (rev 20519)
@@ -1,6 +1,6 @@
all: installed
-SVN_REVISION=-r 30247
+SVN_REVISION=-r 30400
SVN_DIR = kdl-svn-trunk
OLD_SVN_DIR = kdl-svn
SVN_URL = http://svn.mech.kuleuven.be/repos/orocos/trunk/kdl
Modified: pkg/trunk/stacks/geometry/kdl/total.patch
===================================================================
--- pkg/trunk/stacks/geometry/kdl/total.patch 2009-08-03 20:07:05 UTC (rev 20518)
+++ pkg/trunk/stacks/geometry/kdl/total.patch 2009-08-03 20:13:35 UTC (rev 20519)
@@ -1,327 +1,6 @@
-Index: tests/kinfamtest.cpp
-===================================================================
---- tests/kinfamtest.cpp (revision 30247)
-+++ tests/kinfamtest.cpp (working copy)
-@@ -24,44 +24,44 @@
- {
- double q;
- Joint j;
-- j=Joint(Joint::None);
-+ j=Joint("Joint 1", Joint::None);
- CPPUNIT_ASSERT_EQUAL(Joint::None,j.getType());
- random(q);
- CPPUNIT_ASSERT_EQUAL(j.pose(q),Frame::Identity());
- random(q);
- CPPUNIT_ASSERT_EQUAL(j.twist(q),Twist::Zero());
- random(q);
-- j=Joint(Joint::RotX);
-+ j=Joint("Joint 2", Joint::RotX);
- CPPUNIT_ASSERT_EQUAL(Joint::RotX,j.getType());
- CPPUNIT_ASSERT_EQUAL(j.pose(q),Frame(Rotation::RotX(q)));
- random(q);
- CPPUNIT_ASSERT_EQUAL(j.twist(q),Twist(Vector::Zero(),Vector(q,0,0)));
- random(q);
-- j=Joint(Joint::RotY);
-+ j=Joint("Joint 3", Joint::RotY);
- CPPUNIT_ASSERT_EQUAL(Joint::RotY,j.getType());
- CPPUNIT_ASSERT_EQUAL(j.pose(q),Frame(Rotation::RotY(q)));
- random(q);
- CPPUNIT_ASSERT_EQUAL(j.twist(q),Twist(Vector::Zero(),Vector(0,q,0)));
- random(q);
-- j=Joint(Joint::RotZ);
-+ j=Joint("Joint 4", Joint::RotZ);
- CPPUNIT_ASSERT_EQUAL(Joint::RotZ,j.getType());
- CPPUNIT_ASSERT_EQUAL(j.pose(q),Frame(Rotation::RotZ(q)));
- random(q);
- CPPUNIT_ASSERT_EQUAL(j.twist(q),Twist(Vector::Zero(),Vector(0,0,q)));
- random(q);
-- j=Joint(Joint::TransX);
-+ j=Joint("Joint 5", Joint::TransX);
- CPPUNIT_ASSERT_EQUAL(Joint::TransX,j.getType());
- CPPUNIT_ASSERT_EQUAL(j.pose(q),Frame(Vector(q,0,0)));
- random(q);
- CPPUNIT_ASSERT_EQUAL(j.twist(q),Twist(Vector(q,0,0),Vector::Zero()));
- random(q);
-- j=Joint(Joint::TransY);
-+ j=Joint("Joint 6", Joint::TransY);
- CPPUNIT_ASSERT_EQUAL(Joint::TransY,j.getType());
- CPPUNIT_ASSERT_EQUAL(j.pose(q),Frame(Vector(0,q,0)));
- random(q);
- CPPUNIT_ASSERT_EQUAL(j.twist(q),Twist(Vector(0,q,0),Vector::Zero()));
- random(q);
-- j=Joint(Joint::TransZ);
-+ j=Joint("Joint 7", Joint::TransZ);
- CPPUNIT_ASSERT_EQUAL(Joint::TransZ,j.getType());
- CPPUNIT_ASSERT_EQUAL(j.pose(q),Frame(Vector(0,0,q)));
- random(q);
-@@ -75,49 +75,49 @@
- double q,qdot;
- Frame f,f1;
- random(f);
-- s = Segment(Joint(Joint::None),f);
-+ s = Segment("Segment 0", Joint("Joint 0", Joint::None),f);
- random(q);
- random(qdot);
- f1=s.getJoint().pose(q)*f;
- CPPUNIT_ASSERT_EQUAL(f1,s.pose(q));
- CPPUNIT_ASSERT_EQUAL(s.getJoint().twist(qdot).RefPoint(f1.p),s.twist(q,qdot));
- random(f);
-- s = Segment(Joint(Joint::RotX),f);
-+ s = Segment("Segment 1", Joint("Joint 1", Joint::RotX),f);
- random(q);
- random(qdot);
- f1=s.getJoint().pose(q)*f;
- CPPUNIT_ASSERT_EQUAL(f1,s.pose(q));
- CPPUNIT_ASSERT_EQUAL(s.getJoint().twist(qdot).RefPoint(f1.p),s.twist(q,qdot));
- random(f);
-- s = Segment(Joint(Joint::RotY),f);
-+ s = Segment("Segment 3", Joint("Joint 3", Joint::RotY),f);
- random(q);
- random(qdot);
- f1=s.getJoint().pose(q)*f;
- CPPUNIT_ASSERT_EQUAL(f1,s.pose(q));
- CPPUNIT_ASSERT_EQUAL(s.getJoint().twist(qdot).RefPoint(f1.p),s.twist(q,qdot));
- random(f);
-- s = Segment(Joint(Joint::RotZ),f);
-+ s = Segment("Segment 4", Joint("Joint 4", Joint::RotZ),f);
- random(q);
- random(qdot);
- f1=s.getJoint().pose(q)*f;
- CPPUNIT_ASSERT_EQUAL(f1,s.pose(q));
- CPPUNIT_ASSERT_EQUAL(s.getJoint().twist(qdot).RefPoint(f1.p),s.twist(q,qdot));
- random(f);
-- s = Segment(Joint(Joint::TransX),f);
-+ s = Segment("Segment 5", Joint("Joint 5", Joint::TransX),f);
- random(q);
- random(qdot);
- f1=s.getJoint().pose(q)*f;
- CPPUNIT_ASSERT_EQUAL(f1,s.pose(q));
- CPPUNIT_ASSERT_EQUAL(s.getJoint().twist(qdot).RefPoint(f1.p),s.twist(q,qdot));
- random(f);
-- s = Segment(Joint(Joint::TransY),f);
-+ s = Segment("Segment 6", Joint("Joint 6", Joint::TransY),f);
- random(q);
- random(qdot);
- f1=s.getJoint().pose(q)*f;
- CPPUNIT_ASSERT_EQUAL(f1,s.pose(q));
- CPPUNIT_ASSERT_EQUAL(s.getJoint().twist(qdot).RefPoint(f1.p),s.twist(q,qdot));
- random(f);
-- s = Segment(Joint(Joint::TransZ),f);
-+ s = Segment("Segment 7", Joint("Joint 7", Joint::TransZ),f);
- random(q);
- random(qdot);
- f1=s.getJoint().pose(q)*f;
-@@ -129,21 +129,21 @@
- {
- Chain chain1;
-
-- chain1.addSegment(Segment(Joint(Joint::RotZ),
-+ chain1.addSegment(Segment("Segment 0", Joint("Joint 0", Joint::RotZ),
- Frame(Vector(0.0,0.0,0.0))));
-- chain1.addSegment(Segment(Joint(Joint::RotX),
-+ chain1.addSegment(Segment("Segment 1", Joint("Joint 1", Joint::RotX),
- Frame(Vector(0.0,0.0,0.9))));
-- chain1.addSegment(Segment(Joint(Joint::RotX),
-+ chain1.addSegment(Segment("Segment 2", Joint("Joint 2", Joint::RotX),
- Frame(Vector(0.0,0.0,1.2))));
-- chain1.addSegment(Segment(Joint(Joint::RotZ),
-+ chain1.addSegment(Segment("Segment 3", Joint("Joint 3", Joint::RotZ),
- Frame(Vector(0.0,0.0,1.5))));
-- chain1.addSegment(Segment(Joint(Joint::RotX),
-+ chain1.addSegment(Segment("Segment 4", Joint("Joint 4", Joint::RotX),
- Frame(Vector(0.0,0.0,0.0))));
-- chain1.addSegment(Segment(Joint(Joint::RotZ),
-+ chain1.addSegment(Segment("Segment 5", Joint("Joint 5", Joint::RotZ),
- Frame(Vector(0.0,0.0,0.4))));
- CPPUNIT_ASSERT_EQUAL(chain1.getNrOfJoints(),(uint)6);
- CPPUNIT_ASSERT_EQUAL(chain1.getNrOfSegments(),(uint)6);
-- chain1.addSegment(Segment(Joint(Joint::None),Frame(Vector(0.0,0.1,0.0))));
-+ chain1.addSegment(Segment("Segment 6", Joint("Joint 6", Joint::None),Frame(Vector(0.0,0.1,0.0))));
- CPPUNIT_ASSERT_EQUAL(chain1.getNrOfJoints(),(uint)6);
- CPPUNIT_ASSERT_EQUAL(chain1.getNrOfSegments(),(uint)7);
-
-@@ -158,50 +158,57 @@
- void KinFamTest::TreeTest()
- {
- Tree tree1;
-- Segment segment1(Joint(Joint::None));
-- Segment segment2(Joint(Joint::RotX),Frame(Vector(0.1,0.2,0.3)));
-- Segment segment3(Joint(Joint::TransZ),Frame(Rotation::RotX(1.57)));
-+ Segment segment1("Segment 1", Joint("Joint 1", Joint::None));
-+ Segment segment2("Segment 2", Joint("Joint 2", Joint::RotX),Frame(Vector(0.1,0.2,0.3)));
-+ Segment segment3("Segment 3", Joint("Joint 3", Joint::TransZ),Frame(Rotation::RotX(1.57)));
-+ Segment segment4("Segment 4", Joint("Joint 4", Joint::RotX),Frame(Vector(0.1,0.2,0.3)));
-+ Segment segment5("Segment 5", Joint("Joint 5", Joint::RotX),Frame(Vector(0.1,0.2,0.3)));
-+ Segment segment6("Segment 6", Joint("Joint 6", Joint::RotX),Frame(Vector(0.1,0.2,0.3)));
-+ Segment segment7("Segment 7", Joint("Joint 7", Joint::RotX),Frame(Vector(0.1,0.2,0.3)));
-
- cout<<tree1<<endl;
-
-- CPPUNIT_ASSERT(tree1.addSegment(segment1,"Segment1","root"));
-- CPPUNIT_ASSERT(tree1.addSegment(segment2,"Segment2","root"));
-- CPPUNIT_ASSERT(tree1.addSegment(segment3,"Segment3","Segment1"));
-- CPPUNIT_ASSERT(tree1.addSegment(segment2,"Segment4","Segment3"));
-- CPPUNIT_ASSERT(!tree1.addSegment(segment1,"Segment5","Segment6"));
-- CPPUNIT_ASSERT(!tree1.addSegment(segment1,"Segment1","Segment4"));
-+ CPPUNIT_ASSERT(tree1.addSegment(segment1,"root"));
-+ CPPUNIT_ASSERT(tree1.addSegment(segment2,"root"));
-+ CPPUNIT_ASSERT(tree1.addSegment(segment3,"Segment 1"));
-+ CPPUNIT_ASSERT(tree1.addSegment(segment4,"Segment 3"));
-+ CPPUNIT_ASSERT(!tree1.addSegment(segment1,"Segment 6"));
-+ CPPUNIT_ASSERT(!tree1.addSegment(segment1,"Segment 4"));
-
- cout<<tree1<<endl;
-
- Tree tree2;
-- CPPUNIT_ASSERT(tree2.addSegment(segment1,"Segment1","root"));
-- CPPUNIT_ASSERT(tree2.addSegment(segment2,"Segment2","root"));
-- CPPUNIT_ASSERT(tree2.addSegment(segment3,"Segment3","Segment1"));
-+ CPPUNIT_ASSERT(tree2.addSegment(segment5,"root"));
-+ CPPUNIT_ASSERT(tree2.addSegment(segment6,"root"));
-+ CPPUNIT_ASSERT(tree2.addSegment(segment7,"Segment 6"));
-
- cout<<tree2<<endl;
-
- Chain chain1;
-- chain1.addSegment(Segment(Joint(Joint::RotZ),
-+ chain1.addSegment(Segment("Segment 8", Joint("Joint 8", Joint::RotZ),
- Frame(Vector(0.0,0.0,0.0))));
-- chain1.addSegment(Segment(Joint(Joint::RotX),
-+ chain1.addSegment(Segment("Segment 9", Joint("Joint 9", Joint::RotX),
- Frame(Vector(0.0,0.0,0.9))));
-- chain1.addSegment(Segment(Joint(Joint::RotX),
-+ chain1.addSegment(Segment("Segment 10", Joint("Joint 10", Joint::RotX),
- Frame(Vector(0.0,0.0,1.2))));
-- chain1.addSegment(Segment(Joint(Joint::RotZ),
-+ chain1.addSegment(Segment("Segment 11", Joint("Joint 11", Joint::RotZ),
- Frame(Vector(0.0,0.0,1.5))));
-- chain1.addSegment(Segment(Joint(Joint::RotX),
-+ chain1.addSegment(Segment("Segment 12", Joint("Joint 12", Joint::RotX),
- Frame(Vector(0.0,0.0,0.0))));
-- chain1.addSegment(Segment(Joint(Joint::RotZ),
-+ chain1.addSegment(Segment("Segment 13", Joint("Joint 13", Joint::RotZ),
- Frame(Vector(0.0,0.0,0.4))));
-
-
-- CPPUNIT_ASSERT(tree2.addChain(chain1,"Chain1","Segment2"));
-+ CPPUNIT_ASSERT(tree2.addChain(chain1, "Segment 6"));
- cout<<tree2<<endl;
-- CPPUNIT_ASSERT(tree1.addTree(tree2,"Tree2","Segment2"));
-+ CPPUNIT_ASSERT(tree1.addTree(tree2, "Segment 2"));
- cout<<tree1<<endl;
-
-- Chain extract_chain1 = tree1.getChain("Segment2", "Segment4");
-- Chain extract_chain2 = tree1.getChain("Segment4", "Segment2");
-+ Chain extract_chain1;
-+ CPPUNIT_ASSERT(tree1.getChain("Segment 2", "Segment 4", extract_chain1));
-+ CPPUNIT_ASSERT(tree1.getChain("Segment 2", "Segment 4", extract_chain1)); // to make sure chain gets cleared
-+ Chain extract_chain2;
-+ CPPUNIT_ASSERT(tree1.getChain("Segment 4", "Segment 2", extract_chain2));
- CPPUNIT_ASSERT(extract_chain1.getNrOfJoints()==extract_chain2.getNrOfJoints());
- CPPUNIT_ASSERT(extract_chain1.getNrOfSegments()==extract_chain2.getNrOfSegments());
- ChainFkSolverPos_recursive solver1(extract_chain1);
-Index: tests/solvertest.cpp
-===================================================================
---- tests/solvertest.cpp (revision 30247)
-+++ tests/solvertest.cpp (working copy)
-@@ -13,66 +13,66 @@
- {
- srand( (unsigned)time( NULL ));
-
-- chain1.addSegment(Segment(Joint(Joint::RotZ),
-+ chain1.addSegment(Segment("Segment 1", Joint("Joint 1", Joint::RotZ),
- Frame(Vector(0.0,0.0,0.0))));
-- chain1.addSegment(Segment(Joint(Joint::RotX),
-+ chain1.addSegment(Segment("Segment 2", Joint("Joint 2", Joint::RotX),
- Frame(Vector(0.0,0.0,0.9))));
-- chain1.addSegment(Segment(Joint(Joint::None),
-+ chain1.addSegment(Segment("Segment 3", Joint("Joint 3", Joint::None),
- Frame(Vector(-0.4,0.0,0.0))));
-- chain1.addSegment(Segment(Joint(Joint::RotX),
-+ chain1.addSegment(Segment("Segment 4", Joint("Joint 4", Joint::RotX),
- Frame(Vector(0.0,0.0,1.2))));
-- chain1.addSegment(Segment(Joint(Joint::None),
-+ chain1.addSegment(Segment("Segment 5", Joint("Joint 5", Joint::None),
- Frame(Vector(0.4,0.0,0.0))));
-- chain1.addSegment(Segment(Joint(Joint::RotZ),
-+ chain1.addSegment(Segment("Segment 6", Joint("Joint 6", Joint::RotZ),
- Frame(Vector(0.0,0.0,1.4))));
-- chain1.addSegment(Segment(Joint(Joint::RotX),
-+ chain1.addSegment(Segment("Segment 7", Joint("Joint 7", Joint::RotX),
- Frame(Vector(0.0,0.0,0.0))));
-- chain1.addSegment(Segment(Joint(Joint::RotZ),
-+ chain1.addSegment(Segment("Segment 8", Joint("Joint 8", Joint::RotZ),
- Frame(Vector(0.0,0.0,0.4))));
-- chain1.addSegment(Segment(Joint(Joint::None),
-+ chain1.addSegment(Segment("Segment 9", Joint("Joint 9", Joint::None),
- Frame(Vector(0.0,0.0,0.0))));
-
-- chain2.addSegment(Segment(Joint(Joint::RotZ),
-+ chain2.addSegment(Segment("Segment 1", Joint("Joint 1", Joint::RotZ),
- Frame(Vector(0.0,0.0,0.5))));
-- chain2.addSegment(Segment(Joint(Joint::RotX),
-+ chain2.addSegment(Segment("Segment 2", Joint("Joint 2", Joint::RotX),
- Frame(Vector(0.0,0.0,0.4))));
-- chain2.addSegment(Segment(Joint(Joint::RotX),
-+ chain2.addSegment(Segment("Segment 3", Joint("Joint 3", Joint::RotX),
- Frame(Vector(0.0,0.0,0.3))));
-- chain2.addSegment(Segment(Joint(Joint::RotX),
-+ chain2.addSegment(Segment("Segment 4", Joint("Joint 4", Joint::RotX),
- Frame(Vector(0.0,0.0,0.2))));
-- chain2.addSegment(Segment(Joint(Joint::RotZ),
-+ chain2.addSegment(Segment("Segment 5", Joint("Joint 5", Joint::RotZ),
- Frame(Vector(0.0,0.0,0.1))));
-
-
-- chain3.addSegment(Segment(Joint(Joint::RotZ),
-+ chain3.addSegment(Segment("Segment 1", Joint("Joint 1", Joint::RotZ),
- Frame(Vector(0.0,0.0,0.0))));
-- chain3.addSegment(Segment(Joint(Joint::RotX),
-+ chain3.addSegment(Segment("Segment 2", Joint("Joint 2", Joint::RotX),
- Frame(Vector(0.0,0.0,0.9))));
-- chain3.addSegment(Segment(Joint(Joint::RotZ),
-+ chain3.addSegment(Segment("Segment 3", Joint("Joint 3", Joint::RotZ),
- Frame(Vector(-0.4,0.0,0.0))));
-- chain3.addSegment(Segment(Joint(Joint::RotX),
-+ chain3.addSegment(Segment("Segment 4", Joint("Joint 4", Joint::RotX),
- Frame(Vector(0.0,0.0,1.2))));
-- chain3.addSegment(Segment(Joint(Joint::None),
-+ chain3.addSegment(Segment("Segment 5", Joint("Joint 5", Joint::None),
- Frame(Vector(0.4,0.0,0.0))));
-- chain3.addSegment(Segment(Joint(Joint::RotZ),
-+ chain3.addSegment(Segment("Segment 6", Joint("Joint 6", Joint::RotZ),
- Frame(Vector(0.0,0.0,1.4))));
-- chain3.addSegment(Segment(Joint(Joint::RotX),
-+ chain3.addSegment(Segment("Segment 7", Joint("Joint 7", Joint::RotX),
- Frame(Vector(0.0,0.0,0.0))));
-- chain3.addSegment(Segment(Joint(Joint::RotZ),
-+ chain3.addSegment(Segment("Segment 8", Joint("Joint 8", Joint::RotZ),
- Frame(Vector(0.0,0.0,0.4))));
-- chain3.addSegment(Segment(Joint(Joint::RotY),
-+ chain3.addSegment(Segment("Segment 9", Joint("Joint 9", Joint::RotY),
- Frame(Vector(0.0,0.0,0.0))));
-
-
-- chain4.addSegment(Segment(Joint(Vector(10,0,0), Vector(1,0,1),Joint::RotAxis),
-+ chain4.addSegment(Segment("Segment 1", Joint("Joint 1", Vector(10,0,0), Vector(1,0,1),Joint::RotAxis),
- Frame(Vector(0.0,0.0,0.5))));
-- chain4.addSegment(Segment(Joint(Vector(0,5,0), Vector(1,0,0),Joint::RotAxis),
-+ chain4.addSegment(Segment("Segment 2", Joint("Joint 2", Vector(0,5,0), Vector(1,0,0),Joint::RotAxis),
- Frame(Vector(0.0,0.0,0.4))));
-- chain4.addSegment(Segment(Joint(Vector(0,0,5), Vector(1,0,4),Joint::RotAxis),
-+ chain4.addSegment(Segment("Segment 3", Joint("Joint 3", Vector(0,0,5), Vector(1,0,4),Joint::RotAxis),
- Frame(Vector(0.0,0.0,0.3))));
-- chain4.addSegment(Segment(Joint(Vector(0,0,0), Vector(1,0,0),Joint::RotAxis),
-+ chain4.addSegment(Segment("Segment 4", Joint("Joint 4", Vector(0,0,0), Vector(1,0,0),Joint::RotAxis),
- Frame(Vector(0.0,0.0,0.2))));
-- chain4.addSegment(Segment(Joint(Vector(0,0,0), Vector(0,0,1),Joint::RotAxis),
-+ chain4.addSegment(Segment("Segment 5", Joint("Joint 5", Vector(0,0,0), Vector(0,0,1),Joint::RotAxis),
- Frame(Vector(0.0,0.0,0.1))));
-
- }
Index: src/tree.hpp
===================================================================
---- src/tree.hpp (revision 30247)
+--- src/tree.hpp (revision 30400)
+++ src/tree.hpp (working copy)
@@ -37,7 +37,7 @@
class TreeElement
@@ -344,71 +23,8 @@
};
};
-@@ -69,13 +69,15 @@
- int nrOfJoints;
- int nrOfSegments;
-
-- bool addTreeRecursive(SegmentMap::const_iterator root, const std::string& tree_name, const std::string& hook_name);
-+ std::string root_name;
-
-+ bool addTreeRecursive(SegmentMap::const_iterator root, const std::string& hook_name);
-+
- public:
- /**
- * The constructor of a tree, a new tree is always empty
+@@ -142,7 +142,9 @@
*/
-- Tree();
-+ Tree(const std::string& root_name="root");
- Tree(const Tree& in);
- Tree& operator= (const Tree& arg);
-
-@@ -84,39 +86,33 @@
- * hook_name as segment_name
- *
- * @param segment new segment to add
-- * @param segment_name name of the new segment
- * @param hook_name name of the segment to connect this
- * segment with.
- *
- * @return false if hook_name could not be found.
- */
-- bool addSegment(const Segment& segment, const std::string& segment_name, const std::string& hook_name);
-+ bool addSegment(const Segment& segment, const std::string& hook_name);
-
- /**
- * Adds a complete chain to the end of the segment with
-- * hook_name as segment_name. Segment i of
-- * the chain will get chain_name+".Segment"+i as segment_name.
-+ * hook_name as segment_name.
- *
-- * @param chain Chain to add
-- * @param chain_name name of the chain
- * @param hook_name name of the segment to connect the chain with.
- *
- * @return false if hook_name could not be found.
- */
-- bool addChain(const Chain& chain, const std::string& chain_name, const std::string& hook_name);
-+ bool addChain(const Chain& chain, const std::string& hook_name);
-
- /**
- * Adds a complete tree to the end of the segment with
-- * hookname as segment_name. The segments of the tree will get
-- * tree_name+segment_name as segment_name.
-+ * hookname as segment_name.
- *
- * @param tree Tree to add
-- * @param tree_name name of the tree
- * @param hook_name name of the segment to connect the tree with
- *
- * @return false if hook_name could not be found
- */
-- bool addTree(const Tree& tree, const std::string& tree_name,const std::string& hook_name);
-+ bool addTree(const Tree& tree, const std::string& hook_name);
-
- /**
- * Request the total number of joints in the tree.\n
-@@ -146,18 +142,30 @@
- */
SegmentMap::const_iterator getSegment(const std::string& segment_name)const
{
- return segments.find(segment_name);
@@ -416,36 +32,12 @@
+ if (it == segments.end()) throw seg_name_ex;
+ else return it;
};
-+ /**
-+ * Request the root segment of the tree
-+ *
-+ * @return constant iterator pointing to the root segment
-+ */
-+ SegmentMap::const_iterator getRootSegment()const
-+ {
-+ return segments.find(root_name);
-+ };
-
/**
- * Request the chain of the tree between chain_root and chain_tip. The chain_root must be an ancester from chain_tip
- *
- * @param chain_root the name of the root segment of the chain
- * @param chain_tip the name of the tip segment of the chain
-+ * @param chain the resulting chain
- *
-- * @return the chain form chain_root to chain_tip, copied from the tree
-+ * @return success or failure
- */
-- Chain getChain(const std::string& chain_root, const std::string& chain_tip)const;
-+ bool getChain(const std::string& chain_root, const std::string& chain_tip, Chain& chain)const;
+ * Request the root segment of the tree
+@@ -173,6 +175,11 @@
-
- const SegmentMap& getSegments()const
-@@ -166,6 +174,12 @@
- }
-
virtual ~Tree(){};
-+
+
+ class segment_name_exception: public std::exception{
+ virtual const char* what() const throw(){
+ return "Segment Name excption";}
@@ -454,601 +46,46 @@
};
}
#endif
-Index: src/segment.cpp
-===================================================================
---- src/segment.cpp (revision 30247)
-+++ src/segment.cpp (working copy)
-@@ -21,20 +21,22 @@
-
- namespace KDL {
-
-- Segment::Segment(const Joint& _joint, const Frame& _f_tip, const RigidBodyInertia& _I):
-+ Segment::Segment(const std::string& _name, const Joint& _joint, const Frame& _f_tip, const RigidBodyInertia& _I):
-+ name(_name),
- joint(_joint),I(_I),
- f_tip(_joint.pose(0).Inverse() * _f_tip)
- {
- }
-
- Segment::Segment(const Segment& in):
-- joint(in.joint),I(in.I),
-+ name(in.name),joint(in.joint),I(in.I),
- f_tip(in.f_tip)
- {
- }
-
- Segment& Segment::operator=(const Segment& arg)
- {
-+ name=arg.name;
- joint=arg.joint;
- I=arg.I;
- f_tip=arg.f_tip;
-Index: src/joint.cpp
-===================================================================
---- src/joint.cpp (revision 30247)
-+++ src/joint.cpp (working copy)
-@@ -24,17 +24,17 @@
- namespace KDL {
-
- // constructor for joint along x,y or z axis, at origin of reference frame
-- Joint::Joint(const JointType& _type, const double& _scale, const double& _offset,
-+ Joint::Joint(const std::string& _name, const JointType& _type, const double& _scale, const double& _offset,
- const double& _inertia, const double& _damping, const double& _stiffness):
-- type(_type),scale(_scale),offset(_offset),inertia(_inertia),damping(_damping),stiffness(_stiffness)
-+ name(_name),type(_type),scale(_scale),offset(_offset),inertia(_inertia),damping(_damping),stiffness(_stiffness)
- {
- if (type == RotAxis || type == TransAxis) throw joint_type_ex;
- }
-
- // constructor for joint along arbitrary axis, at arbitrary origin
-- Joint::Joint(const Vector& _origin, const Vector& _axis, const JointType& _type, const double& _scale, const double& _offset,
-- const double& _inertia, const double& _damping, const double& _stiffness):
-- origin(_origin), axis(_axis / _axis.Norm()), type(_type),scale(_scale),offset(_offset),inertia(_inertia),damping(_damping),stiffness(_stiffness)
-+ Joint::Joint(const std::string& _name, const Vector& _origin, const Vector& _axis, const JointType& _type, const double& _scale,
-+ const double& _offset, const double& _inertia, const double& _damping, const double& _stiffness):
-+ name(_name), origin(_origin), axis(_axis / _axis.Norm()), type(_type),scale(_scale),offset(_offset),inertia(_inertia),damping(_damping),stiffness(_stiffness)
- {
- if (type != RotAxis && type != TransAxis) throw joint_type_ex;
-
Index: src/tree.cpp
===================================================================
---- src/tree.cpp (revision 30247)
+--- src/tree.cpp (revision 30400)
+++ src/tree.cpp (working copy)
-@@ -25,18 +25,19 @@
- namespace KDL {
- using namespace std;
+@@ -27,7 +27,7 @@
--Tree::Tree() :
-- nrOfSegments(0), nrOfJoints(0) {
-- segments.insert(make_pair("root", TreeElement::Root()));
-+Tree::Tree(const std::string& _root_name) :
-+ nrOfSegments(0), nrOfJoints(0),root_name(_root_name) {
+ Tree::Tree(const std::string& _root_name) :
+ nrOfSegments(0), nrOfJoints(0),root_name(_root_name) {
+- segments.insert(make_pair(root_name, TreeElement::Root()));
+ segments.insert(make_pair(root_name, TreeElement::Root(root_name)));
}
Tree::Tree(const Tree& in) {
- segments.clear();
- nrOfSegments = 0;
+@@ -36,9 +36,8 @@
nrOfJoints = 0;
-+ root_name = in.root_name;
+ root_name = in.root_name;
-- segments.insert(make_pair("root", TreeElement::Root()));
-- this->addTree(in, "", "root");
+- segments.insert(make_pair(root_name, TreeElement::Root()));
+ segments.insert(make_pair(root_name, TreeElement::Root(root_name)));
-+ this->addTree(in, root_name);
-
+ this->addTree(in, root_name);
+-
}
-@@ -44,21 +45,21 @@
- segments.clear();
- nrOfSegments = 0;
+ Tree& Tree::operator=(const Tree& in) {
+@@ -47,7 +46,7 @@
nrOfJoints = 0;
-+ root_name = in.root_name;
+ root_name = in.root_name;
-- segments.insert(make_pair("root", TreeElement::Root()));
-- this->addTree(in, "", "root");
+- segments.insert(make_pair(in.root_name, TreeElement::Root()));
+ segments.insert(make_pair(in.root_name, TreeElement::Root(root_name)));
-+ this->addTree(in, root_name);
+ this->addTree(in, root_name);
return *this;
}
+@@ -110,6 +109,9 @@
--bool Tree::addSegment(const Segment& segment, const std::string& segment_name,
-- const std::string& hook_name) {
-+bool Tree::addSegment(const Segment& segment, const std::string& hook_name) {
- SegmentMap::iterator parent = segments.find(hook_name);
- //check if parent exists
- if (parent == segments.end())
- return false;
- pair<SegmentMap::iterator, bool> retval;
- //insert new element
-- retval = segments.insert(make_pair(segment_name, TreeElement(segment,
-+ retval = segments.insert(make_pair(segment.getName()...
[truncated message content] |
|
From: <is...@us...> - 2009-08-03 23:15:01
|
Revision: 20559
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=20559&view=rev
Author: isucan
Date: 2009-08-03 23:14:44 +0000 (Mon, 03 Aug 2009)
Log Message:
-----------
switching from mechanism_state to joint_states
Modified Paths:
--------------
pkg/trunk/highlevel/move_arm/src/arm_cmd_line.cpp
pkg/trunk/highlevel/move_arm/src/move_arm_action.cpp
pkg/trunk/motion_planning/ompl_planning/src/request_handler/RequestHandler.cpp
pkg/trunk/motion_planning/planning_environment/include/planning_environment/monitors/kinematic_model_state_monitor.h
pkg/trunk/motion_planning/planning_environment/mainpage.dox
pkg/trunk/motion_planning/planning_environment/src/monitors/kinematic_model_state_monitor.cpp
pkg/trunk/motion_planning/planning_environment/src/monitors/planning_monitor.cpp
pkg/trunk/motion_planning/planning_environment/src/tools/display_planner_collision_model.cpp
Modified: pkg/trunk/highlevel/move_arm/src/arm_cmd_line.cpp
===================================================================
--- pkg/trunk/highlevel/move_arm/src/arm_cmd_line.cpp 2009-08-03 23:08:40 UTC (rev 20558)
+++ pkg/trunk/highlevel/move_arm/src/arm_cmd_line.cpp 2009-08-03 23:14:44 UTC (rev 20559)
@@ -219,7 +219,7 @@
motion_planning_msgs::KinematicPath kp;
kp.header.frame_id = km.getFrameId();
- kp.header.stamp = km.lastMechanismStateUpdate();
+ kp.header.stamp = km.lastJointStateUpdate();
// fill in start state with current one
std::vector<planning_models::KinematicModel::Joint*> joints;
@@ -229,7 +229,7 @@
for (unsigned int i = 0 ; i < joints.size() ; ++i)
{
kp.start_state[i].header.frame_id = km.getFrameId();
- kp.start_state[i].header.stamp = km.lastMechanismStateUpdate();
+ kp.start_state[i].header.stamp = km.lastJointStateUpdate();
kp.start_state[i].joint_name = joints[i]->name;
st.copyParamsJoint(kp.start_state[i].value, joints[i]->name);
}
Modified: pkg/trunk/highlevel/move_arm/src/move_arm_action.cpp
===================================================================
--- pkg/trunk/highlevel/move_arm/src/move_arm_action.cpp 2009-08-03 23:08:40 UTC (rev 20558)
+++ pkg/trunk/highlevel/move_arm/src/move_arm_action.cpp 2009-08-03 23:14:44 UTC (rev 20559)
@@ -542,7 +542,7 @@
for (unsigned int i = 0 ; i < joints.size() ; ++i)
{
start_state[i].header.frame_id = planningMonitor_->getFrameId();
- start_state[i].header.stamp = planningMonitor_->lastMechanismStateUpdate();
+ start_state[i].header.stamp = planningMonitor_->lastJointStateUpdate();
start_state[i].joint_name = joints[i]->name;
st.copyParamsJoint(start_state[i].value, joints[i]->name);
}
@@ -591,7 +591,7 @@
motion_planning_msgs::JointConstraint jc;
jc.joint_name = arm_joint_names_[i];
jc.header.frame_id = req.goal_constraints.pose_constraint[0].pose.header.frame_id;
- jc.header.stamp = planningMonitor_->lastMechanismStateUpdate();
+ jc.header.stamp = planningMonitor_->lastJointStateUpdate();
unsigned int u = planningMonitor_->getKinematicModel()->getJoint(arm_joint_names_[i])->usedParams;
for (unsigned int j = 0 ; j < u ; ++j)
{
Modified: pkg/trunk/motion_planning/ompl_planning/src/request_handler/RequestHandler.cpp
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/src/request_handler/RequestHandler.cpp 2009-08-03 23:08:40 UTC (rev 20558)
+++ pkg/trunk/motion_planning/ompl_planning/src/request_handler/RequestHandler.cpp 2009-08-03 23:14:44 UTC (rev 20559)
@@ -643,7 +643,7 @@
return;
visualization_msgs::Marker mk;
- mk.header.stamp = psetup->model->planningMonitor->lastMechanismStateUpdate();
+ mk.header.stamp = psetup->model->planningMonitor->lastJointStateUpdate();
mk.header.frame_id = psetup->model->planningMonitor->getFrameId();
mk.ns = ros::this_node::getName();
mk.id = 1;
Modified: pkg/trunk/motion_planning/planning_environment/include/planning_environment/monitors/kinematic_model_state_monitor.h
===================================================================
--- pkg/trunk/motion_planning/planning_environment/include/planning_environment/monitors/kinematic_model_state_monitor.h 2009-08-03 23:08:40 UTC (rev 20558)
+++ pkg/trunk/motion_planning/planning_environment/include/planning_environment/monitors/kinematic_model_state_monitor.h 2009-08-03 23:14:44 UTC (rev 20559)
@@ -41,7 +41,7 @@
#include <tf/transform_datatypes.h>
#include <tf/transform_listener.h>
#include <tf/message_notifier.h>
-#include <mechanism_msgs/MechanismState.h>
+#include <mechanism_msgs/JointStates.h>
#include <mapping_msgs/AttachedObject.h>
#include <boost/bind.hpp>
#include <vector>
@@ -143,16 +143,16 @@
return robot_frame_;
}
- /** \brief Return true if a full mechanism state has been received (including pose, if pose inclusion is enabled) */
+ /** \brief Return true if a full joint state has been received (including pose, if pose inclusion is enabled) */
bool haveState(void) const
{
- return haveMechanismState_ && (!includePose_ || (includePose_ && havePose_));
+ return haveJointState_ && (!includePose_ || (includePose_ && havePose_));
}
/** \brief Return the time of the last state update */
- const ros::Time& lastMechanismStateUpdate(void) const
+ const ros::Time& lastJointStateUpdate(void) const
{
- return lastMechanismStateUpdate_;
+ return lastJointStateUpdate_;
}
/** \brief Return the time of the last pose update */
@@ -161,11 +161,11 @@
return lastPoseUpdate_;
}
- /** \brief Wait until a full mechanism state is received (including pose, if pose inclusion is enabled) */
+ /** \brief Wait until a full joint state is received (including pose, if pose inclusion is enabled) */
void waitForState(void) const;
- /** \brief Return true if a mechanism state has been received in the last sec seconds. If sec < 10us, this function always returns true. */
- bool isMechanismStateUpdated(double sec) const;
+ /** \brief Return true if a joint state has been received in the last sec seconds. If sec < 10us, this function always returns true. */
+ bool isJointStateUpdated(double sec) const;
/** \brief Return true if a pose has been received in the last
sec seconds. If sec < 10us, this function always returns
@@ -184,7 +184,7 @@
protected:
void setupRSM(void);
- void mechanismStateCallback(const mechanism_msgs::MechanismStateConstPtr &mechanismState);
+ void jointStateCallback(const mechanism_msgs::JointStatesConstPtr &jointState);
void attachObjectCallback(const mapping_msgs::AttachedObjectConstPtr &attachedObject);
virtual bool attachObject(const mapping_msgs::AttachedObjectConstPtr &attachedObject);
@@ -197,7 +197,7 @@
bool stateMonitorStarted_;
ros::NodeHandle nh_;
- ros::Subscriber mechanismStateSubscriber_;
+ ros::Subscriber jointStateSubscriber_;
tf::TransformListener *tf_;
tf::MessageNotifier<mapping_msgs::AttachedObject>
@@ -213,8 +213,8 @@
onAfterAttachBody_;
bool havePose_;
- bool haveMechanismState_;
- ros::Time lastMechanismStateUpdate_;
+ bool haveJointState_;
+ ros::Time lastJointStateUpdate_;
ros::Time lastPoseUpdate_;
};
Modified: pkg/trunk/motion_planning/planning_environment/mainpage.dox
===================================================================
--- pkg/trunk/motion_planning/planning_environment/mainpage.dox 2009-08-03 23:08:40 UTC (rev 20558)
+++ pkg/trunk/motion_planning/planning_environment/mainpage.dox 2009-08-03 23:14:44 UTC (rev 20559)
@@ -176,7 +176,7 @@
<hr>
The monitor classes are:
-- \b planning_environment::KinematicModelStateMonitor : monitors the kinematic state of the robot. Optionally, monitors the base location. It uses the 'mechanism_state' topic.
+- \b planning_environment::KinematicModelStateMonitor : monitors the kinematic state of the robot. Optionally, monitors the base location. It uses the 'joint_states' topic.
- \b planning_environment::CollisionSpaceMonitor : same as \b planning_environment::KinematicModelStateMonitor except it also monitors the state of the collision environment. It uses the 'collision_map' topic to receive new full maps and the 'collision_map_update' to receive updates. Attaching objects to robot links is possible using the 'attach_object' topic.
- \b planning_environment::PlanningMonitor : same as \b planning_environment::CollisionSpaceMonitor except it also offers the ability to evaluate kinematic constraints and check paths and states for validity.
@@ -185,7 +185,7 @@
\subsection topics ROS topics
Subscribes to:
- - @b "mechanism_state"/MechanismState : the parameters describing the robot's current state
+ - @b "joint_states"/JointStates : the parameters describing the robot's current state
- @b "collision_map"/CollisionMap : data describing the 3D environment
- @b "collision_map_update"/CollisionMap : updates (additive) to data describing the 3D environment
- @b "object_in_map"/ObjectInMap : definition of an object identified in the environment (to be used for collision checking)
Modified: pkg/trunk/motion_planning/planning_environment/src/monitors/kinematic_model_state_monitor.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_environment/src/monitors/kinematic_model_state_monitor.cpp 2009-08-03 23:08:40 UTC (rev 20558)
+++ pkg/trunk/motion_planning/planning_environment/src/monitors/kinematic_model_state_monitor.cpp 2009-08-03 23:14:44 UTC (rev 20559)
@@ -48,7 +48,7 @@
onStateUpdate_ = NULL;
onAfterAttachBody_ = NULL;
attachedBodyNotifier_ = NULL;
- havePose_ = haveMechanismState_ = false;
+ havePose_ = haveJointState_ = false;
if (rm_->loadedModels())
{
@@ -88,8 +88,8 @@
if (rm_->loadedModels())
{
- mechanismStateSubscriber_ = nh_.subscribe("mechanism_state", 1, &KinematicModelStateMonitor::mechanismStateCallback, this);
- ROS_DEBUG("Listening to mechanism state");
+ jointStateSubscriber_ = nh_.subscribe("joint_states", 1, &KinematicModelStateMonitor::jointStateCallback, this);
+ ROS_DEBUG("Listening to joint states");
attachedBodyNotifier_ = new tf::MessageNotifier<mapping_msgs::AttachedObject>(*tf_, boost::bind(&KinematicModelStateMonitor::attachObjectCallback, this, _1), "attach_object", "", 1);
attachedBodyNotifier_->setTargetFrame(rm_->getCollisionCheckLinks());
@@ -106,53 +106,53 @@
delete attachedBodyNotifier_;
attachedBodyNotifier_ = NULL;
- mechanismStateSubscriber_.shutdown();
+ jointStateSubscriber_.shutdown();
ROS_DEBUG("Kinematic state is no longer being monitored");
stateMonitorStarted_ = false;
}
-void planning_environment::KinematicModelStateMonitor::mechanismStateCallback(const mechanism_msgs::MechanismStateConstPtr &mechanismState)
+void planning_environment::KinematicModelStateMonitor::jointStateCallback(const mechanism_msgs::JointStatesConstPtr &jointState)
{
- bool change = !haveMechanismState_;
+ bool change = !haveJointState_;
static bool first_time = true;
- unsigned int n = mechanismState->get_joint_states_size();
+ unsigned int n = jointState->get_joints_size();
for (unsigned int i = 0 ; i < n ; ++i)
{
- planning_models::KinematicModel::Joint* joint = kmodel_->getJoint(mechanismState->joint_states[i].name);
+ planning_models::KinematicModel::Joint* joint = kmodel_->getJoint(jointState->joints[i].name);
if (joint)
{
if (joint->usedParams == 1)
{
- double pos = mechanismState->joint_states[i].position;
+ double pos = jointState->joints[i].position;
planning_models::KinematicModel::RevoluteJoint* rjoint = dynamic_cast<planning_models::KinematicModel::RevoluteJoint*>(joint);
if (rjoint)
if (rjoint->continuous)
pos = angles::normalize_angle(pos);
- bool this_changed = robotState_->setParamsJoint(&pos, mechanismState->joint_states[i].name);
+ bool this_changed = robotState_->setParamsJoint(&pos, jointState->joints[i].name);
change = change || this_changed;
}
else
{
if (first_time)
- ROS_WARN("Incorrect number of parameters: %s (expected %d, had 1)", mechanismState->joint_states[i].name.c_str(), joint->usedParams);
+ ROS_WARN("Incorrect number of parameters: %s (expected %d, had 1)", jointState->joints[i].name.c_str(), joint->usedParams);
}
}
else
{
if (first_time)
- ROS_ERROR("Unknown joint: %s", mechanismState->joint_states[i].name.c_str());
+ ROS_ERROR("Unknown joint: %s", jointState->joints[i].name.c_str());
}
}
first_time = false;
- lastMechanismStateUpdate_ = mechanismState->header.stamp;
- if (!haveMechanismState_)
- haveMechanismState_ = robotState_->seenAll();
+ lastJointStateUpdate_ = jointState->header.stamp;
+ if (!haveJointState_)
+ haveJointState_ = robotState_->seenAll();
if (includePose_)
{
@@ -301,13 +301,13 @@
ROS_INFO("Robot state received!");
}
-bool planning_environment::KinematicModelStateMonitor::isMechanismStateUpdated(double sec) const
+bool planning_environment::KinematicModelStateMonitor::isJointStateUpdated(double sec) const
{
- if (!haveMechanismState_)
+ if (!haveJointState_)
return false;
// less than 10us is considered 0
- if (sec > 1e-5 && lastMechanismStateUpdate_ < ros::Time::now() - ros::Duration(sec))
+ if (sec > 1e-5 && lastJointStateUpdate_ < ros::Time::now() - ros::Duration(sec))
return false;
else
return true;
Modified: pkg/trunk/motion_planning/planning_environment/src/monitors/planning_monitor.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_environment/src/monitors/planning_monitor.cpp 2009-08-03 23:08:40 UTC (rev 20558)
+++ pkg/trunk/motion_planning/planning_environment/src/monitors/planning_monitor.cpp 2009-08-03 23:14:44 UTC (rev 20559)
@@ -53,7 +53,7 @@
return false;
}
- if (!isMechanismStateUpdated(intervalState_))
+ if (!isJointStateUpdated(intervalState_))
{
ROS_WARN("Planning is not safe: robot state not updated in the last %f seconds", intervalState_);
return false;
Modified: pkg/trunk/motion_planning/planning_environment/src/tools/display_planner_collision_model.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_environment/src/tools/display_planner_collision_model.cpp 2009-08-03 23:08:40 UTC (rev 20558)
+++ pkg/trunk/motion_planning/planning_environment/src/tools/display_planner_collision_model.cpp 2009-08-03 23:14:44 UTC (rev 20559)
@@ -131,7 +131,7 @@
void afterAttachBody(planning_models::KinematicModel::Link *link, const mapping_msgs::AttachedObjectConstPtr &attachedObject)
{
roslib::Header header;
- header.stamp = collisionSpaceMonitor_->lastMechanismStateUpdate();
+ header.stamp = collisionSpaceMonitor_->lastJointStateUpdate();
header.frame_id = link->name;
for (unsigned int i = 0 ; i < link->attachedBodies.size() ; ++i)
{
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <is...@us...> - 2009-08-04 01:28:54
|
Revision: 20600
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=20600&view=rev
Author: isucan
Date: 2009-08-04 01:28:47 +0000 (Tue, 04 Aug 2009)
Log Message:
-----------
option to get all collision contacts
Modified Paths:
--------------
pkg/trunk/motion_planning/planning_environment/include/planning_environment/monitors/planning_monitor.h
pkg/trunk/motion_planning/planning_environment/src/monitors/planning_monitor.cpp
pkg/trunk/world_models/collision_space/include/collision_space/environment.h
pkg/trunk/world_models/collision_space/src/environmentBullet.cpp
pkg/trunk/world_models/collision_space/src/environmentODE.cpp
Modified: pkg/trunk/motion_planning/planning_environment/include/planning_environment/monitors/planning_monitor.h
===================================================================
--- pkg/trunk/motion_planning/planning_environment/include/planning_environment/monitors/planning_monitor.h 2009-08-04 01:28:33 UTC (rev 20599)
+++ pkg/trunk/motion_planning/planning_environment/include/planning_environment/monitors/planning_monitor.h 2009-08-04 01:28:47 UTC (rev 20600)
@@ -55,13 +55,15 @@
PlanningMonitor(CollisionModels *cm, tf::TransformListener *tf, std::string frame_id) : CollisionSpaceMonitor(static_cast<CollisionModels*>(cm), tf, frame_id)
{
- onCollisionContact_ = NULL;
+ onCollisionContact_ = NULL;
+ maxCollisionContacts_ = 1;
loadParams();
}
PlanningMonitor(CollisionModels *cm, tf::TransformListener *tf) : CollisionSpaceMonitor(static_cast<CollisionModels*>(cm), tf)
{
onCollisionContact_ = NULL;
+ maxCollisionContacts_ = 1;
loadParams();
}
@@ -124,9 +126,10 @@
bool transformJointToFrame(motion_planning_msgs::KinematicJoint &kj, const std::string &target) const;
/** \brief Set a callback to be called when a collision is found */
- void setOnCollisionContactCallback(const boost::function<void(collision_space::EnvironmentModel::Contact&)> &callback)
+ void setOnCollisionContactCallback(const boost::function<void(collision_space::EnvironmentModel::Contact&)> &callback, unsigned int maxContacts = 1)
{
onCollisionContact_ = callback;
+ maxCollisionContacts_ = maxContacts;
}
protected:
@@ -145,6 +148,7 @@
/** \brief User callback when a collision is found */
boost::function<void(collision_space::EnvironmentModel::Contact&)> onCollisionContact_;
+ unsigned int maxCollisionContacts_;
motion_planning_msgs::KinematicConstraints kcPath_;
motion_planning_msgs::KinematicConstraints kcGoal_;
Modified: pkg/trunk/motion_planning/planning_environment/src/monitors/planning_monitor.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_environment/src/monitors/planning_monitor.cpp 2009-08-04 01:28:33 UTC (rev 20599)
+++ pkg/trunk/motion_planning/planning_environment/src/monitors/planning_monitor.cpp 2009-08-04 01:28:47 UTC (rev 20600)
@@ -298,7 +298,7 @@
// check for collision
std::vector<collision_space::EnvironmentModel::Contact> contacts;
- bool valid = !getEnvironmentModel()->getCollisionContacts(contacts, 1);
+ bool valid = !getEnvironmentModel()->getCollisionContacts(contacts, maxCollisionContacts_);
getKinematicModel()->unlock();
getEnvironmentModel()->unlock();
Modified: pkg/trunk/world_models/collision_space/include/collision_space/environment.h
===================================================================
--- pkg/trunk/world_models/collision_space/include/collision_space/environment.h 2009-08-04 01:28:33 UTC (rev 20599)
+++ pkg/trunk/world_models/collision_space/include/collision_space/environment.h 2009-08-04 01:28:47 UTC (rev 20600)
@@ -139,7 +139,7 @@
/** \brief Check for self collision. Contacts are not computed */
virtual bool isSelfCollision(void) = 0;
- /** \brief Get the list of contacts (collisions) */
+ /** \brief Get the list of contacts (collisions). The maximum number of contacts to be returned can be specified. If the value is 0, all found contacts are returned. */
virtual bool getCollisionContacts(std::vector<Contact> &contacts, unsigned int max_count = 1) = 0;
Modified: pkg/trunk/world_models/collision_space/src/environmentBullet.cpp
===================================================================
--- pkg/trunk/world_models/collision_space/src/environmentBullet.cpp 2009-08-04 01:28:33 UTC (rev 20599)
+++ pkg/trunk/world_models/collision_space/src/environmentBullet.cpp 2009-08-04 01:28:47 UTC (rev 20600)
@@ -246,7 +246,7 @@
if (nc > 0)
{
collision = true;
- for (int j = 0 ; j < nc && contacts.size() < max_count ; ++j)
+ for (int j = 0 ; j < nc && (contacts.size() < max_count || max_count == 0) ; ++j)
{
btManifoldPoint& pt = contactManifold->getContactPoint(j);
collision_space::EnvironmentModelBullet::Contact add;
@@ -268,7 +268,7 @@
}
contacts.push_back(add);
}
- if (contacts.size() >= max_count)
+ if (max_count > 0 && contacts.size() >= max_count)
break;
}
}
Modified: pkg/trunk/world_models/collision_space/src/environmentODE.cpp
===================================================================
--- pkg/trunk/world_models/collision_space/src/environmentODE.cpp 2009-08-04 01:28:33 UTC (rev 20599)
+++ pkg/trunk/world_models/collision_space/src/environmentODE.cpp 2009-08-04 01:28:47 UTC (rev 20600)
@@ -514,7 +514,7 @@
cdata->collides = true;
for (int i = 0 ; i < numc ; ++i)
{
- if (cdata->contacts->size() < cdata->max_contacts)
+ if (cdata->max_contacts == 0 || cdata->contacts->size() < cdata->max_contacts)
{
collision_space::EnvironmentModelODE::Contact add;
@@ -537,7 +537,7 @@
break;
}
}
- if (cdata->contacts->size() >= cdata->max_contacts)
+ if (cdata->max_contacts > 0 && cdata->contacts->size() >= cdata->max_contacts)
cdata->done = true;
}
else
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <tf...@us...> - 2009-08-04 01:40:18
|
Revision: 20606
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=20606&view=rev
Author: tfoote
Date: 2009-08-04 01:40:10 +0000 (Tue, 04 Aug 2009)
Log Message:
-----------
Fixing 3rdparty Makefiles to be robust to calls with -C
Modified Paths:
--------------
pkg/trunk/3rdparty/libsunflower/Makefile
pkg/trunk/3rdparty/nepumuk/Makefile
pkg/trunk/3rdparty/ocl/Makefile
pkg/trunk/3rdparty/player/Makefile
pkg/trunk/3rdparty/rtt/Makefile
pkg/trunk/openrave_planning/openrave/Makefile
pkg/trunk/openrave_planning/soqt/Makefile
pkg/trunk/stacks/camera_drivers/libdc1394v2/Makefile
pkg/trunk/stacks/common/bfl/Makefile
pkg/trunk/stacks/laser_drivers/sicktoolbox/Makefile
Modified: pkg/trunk/3rdparty/libsunflower/Makefile
===================================================================
--- pkg/trunk/3rdparty/libsunflower/Makefile 2009-08-04 01:38:17 UTC (rev 20605)
+++ pkg/trunk/3rdparty/libsunflower/Makefile 2009-08-04 01:40:10 UTC (rev 20606)
@@ -23,13 +23,13 @@
TARBALL= build/libsunflower-$(TARBALL_VERSION).tar.gz
TARBALL_URL= http://pr.willowgarage.com/downloads/libsunflower-$(TARBALL_VERSION).tar.gz
-SOURCE_DIR= $(PWD)/build/libsunflower-$(TARBALL_VERSION)
+SOURCE_DIR= build/libsunflower-$(TARBALL_VERSION)
UNPACK_CMD= tar xfz
-BUILD_DIR= $(PWD)/build
-INST_DIR= $(PWD)/local
+BUILD_DIR= build
+INST_DIR= local
BOOST_DIR= `rosboost-cfg --include_dirs`
-SVN_DIR= $(PWD)/build/sunflower-svn
+SVN_DIR= build/sunflower-svn
SVN_URL= https://libsunflower.svn.sourceforge.net/svnroot/libsunflower/trunk/sunflower
#include $(shell rospack find mk)/download_unpack_build.mk
@@ -38,8 +38,8 @@
installed: $(SVN_DIR) patched Makefile
cd $(SVN_DIR) && ./bootstrap-buildsystem.sh
test -d $(BUILD_DIR) || mkdir -p $(BUILD_DIR)
- cd $(BUILD_DIR) && $(SVN_DIR)/configure --with-pic \
- --prefix=$(INST_DIR) \
+ cd $(BUILD_DIR) && `rospack find libsunflower`/$(SVN_DIR)/configure --with-pic \
+ --prefix=`rospack find libsunflower`/$(INST_DIR) \
--with-boost=$(BOOST_DIR)
$(MAKE) -C $(BUILD_DIR) install
touch installed
Modified: pkg/trunk/3rdparty/nepumuk/Makefile
===================================================================
--- pkg/trunk/3rdparty/nepumuk/Makefile 2009-08-04 01:38:17 UTC (rev 20605)
+++ pkg/trunk/3rdparty/nepumuk/Makefile 2009-08-04 01:40:10 UTC (rev 20606)
@@ -1,9 +1,9 @@
all: installed
SVN_REVISION= -r 1053
-BUILD_DIR= $(PWD)/build
-INST_DIR= $(PWD)/local
-SVN_DIR= $(PWD)/build/nepumuk-svn
+BUILD_DIR= build
+INST_DIR= local
+SVN_DIR= build/nepumuk-svn
SVN_URL= https://libsunflower.svn.sourceforge.net/svnroot/libsunflower/trunk/nepumuk
##later## ESTAR_DIR= $(shell rospack find estar)/local
@@ -18,7 +18,7 @@
installed: $(SVN_DIR) patched
cd $(SVN_DIR) && ./bootstrap-buildsystem.sh
test -d $(BUILD_DIR) || mkdir -p $(BUILD_DIR)
- cd $(BUILD_DIR) && $(SVN_DIR)/configure --prefix=$(INST_DIR) \
+ cd $(BUILD_DIR) && `rospack find nepumuk`/$(SVN_DIR)/configure --prefix=`rospack find nepumuk`/$(INST_DIR) \
--with-sfl=$(SFL_DIR) \
--with-boost=$(BOOST_DIR)
$(MAKE) -C $(BUILD_DIR) install
Modified: pkg/trunk/3rdparty/ocl/Makefile
===================================================================
--- pkg/trunk/3rdparty/ocl/Makefile 2009-08-04 01:38:17 UTC (rev 20605)
+++ pkg/trunk/3rdparty/ocl/Makefile 2009-08-04 01:40:10 UTC (rev 20606)
@@ -13,7 +13,7 @@
KDL_INSTALL=`rospack find kdl`/kdl
BFL_INSTALL=`rospack find bfl`/bfl-boost
-CMAKE_ARGS = -DCMAKE_INSTALL_PREFIX=$(PWD)/$(INSTALL_DIR)/ \
+CMAKE_ARGS = -DCMAKE_INSTALL_PREFIX=`rospack find ocl`/$(INSTALL_DIR)/ \
-DOROCOS_TARGET=gnulinux \
-DCMAKE_BUILD_TYPE="RTT" \
-DOROCOS_INSTALL=$(OROCOS_INSTALL) \
Modified: pkg/trunk/3rdparty/player/Makefile
===================================================================
--- pkg/trunk/3rdparty/player/Makefile 2009-08-04 01:38:17 UTC (rev 20605)
+++ pkg/trunk/3rdparty/player/Makefile 2009-08-04 01:40:10 UTC (rev 20606)
@@ -18,7 +18,7 @@
CMAKE = cmake
CMAKE_ARGS = -D CMAKE_BUILD_TYPE=RELEASE \
- -D CMAKE_INSTALL_PREFIX=$(PWD)/player \
+ -D CMAKE_INSTALL_PREFIX=`rospack find player`/player \
-D INCLUDE_RTK=OFF \
-D BUILD_PLAYERCC=OFF \
-D BUILD_UTILS=OFF \
Modified: pkg/trunk/3rdparty/rtt/Makefile
===================================================================
--- pkg/trunk/3rdparty/rtt/Makefile 2009-08-04 01:38:17 UTC (rev 20605)
+++ pkg/trunk/3rdparty/rtt/Makefile 2009-08-04 01:40:10 UTC (rev 20606)
@@ -6,7 +6,7 @@
INSTALL_DIR = rtt
CMAKE = cmake
BOOST_INCLUDE =$(shell rosboost-cfg --include_dirs)
-CMAKE_ARGS = -DCMAKE_INSTALL_PREFIX=$(PWD)/$(INSTALL_DIR)/ \
+CMAKE_ARGS = -DCMAKE_INSTALL_PREFIX=`rospack find rtt`/$(INSTALL_DIR)/ \
-DOROCOS_TARGET=gnulinux \
-DBOOST_DIR=$(BOOST_INCLUDE)\
-DHAS_BOOST_SPIRIT=$(BOOST_INCLUDE)\
Modified: pkg/trunk/openrave_planning/openrave/Makefile
===================================================================
--- pkg/trunk/openrave_planning/openrave/Makefile 2009-08-04 01:38:17 UTC (rev 20605)
+++ pkg/trunk/openrave_planning/openrave/Makefile 2009-08-04 01:40:10 UTC (rev 20606)
@@ -29,7 +29,7 @@
Version: 2.74\n\
Libs: ${BULLET_LFLAGS}\n\
CFlags: ${BULLET_CFLAGS}" > bullet.pc
- cd $(SVN_DIR) && export PKG_CONFIG_PATH="$(PKG_CONFIG_PATH):$(PWD)" && export PATH="$(shell rospack find soqt)/bin:$(shell rospack find opende)/opende/bin:$(PATH)" && mkdir -p build && cd build && BOOST_INCLUDEDIR=$(BOOST_INCLUDE_DIRS) BOOST_LIBRARYDIR=$(BOOST_LIBRARY_DIRS) cmake -DCMAKE_INSTALL_PREFIX=$(PWD) -DCMAKE_BUILD_TYPE=RelWithDebInfo -DBoost_INCLUDE_DIR=$(BOOST_INCLUDE_DIRS) -DBoost_LIBRARY_DIR=$(BOOST_LIBRARY_DIRS) .. && export PARALLEL_JOBS=ROS_PARALLEL_JOBS && make $(ROS_PARALLEL_JOBS) install
+ cd $(SVN_DIR) && export PKG_CONFIG_PATH="$(PKG_CONFIG_PATH):`rospack find openrave`" && export PATH="$(shell rospack find soqt)/bin:$(shell rospack find opende)/opende/bin:$(PATH)" && mkdir -p build && cd build && BOOST_INCLUDEDIR=$(BOOST_INCLUDE_DIRS) BOOST_LIBRARYDIR=$(BOOST_LIBRARY_DIRS) cmake -DCMAKE_INSTALL_PREFIX=`rospack find openrave` -DCMAKE_BUILD_TYPE=RelWithDebInfo -DBoost_INCLUDE_DIR=$(BOOST_INCLUDE_DIRS) -DBoost_LIBRARY_DIR=$(BOOST_LIBRARY_DIRS) .. && export PARALLEL_JOBS=ROS_PARALLEL_JOBS && make $(ROS_PARALLEL_JOBS) install
touch installed
clean:
Modified: pkg/trunk/openrave_planning/soqt/Makefile
===================================================================
--- pkg/trunk/openrave_planning/soqt/Makefile 2009-08-04 01:38:17 UTC (rev 20605)
+++ pkg/trunk/openrave_planning/soqt/Makefile 2009-08-04 01:40:10 UTC (rev 20606)
@@ -11,7 +11,7 @@
installed: wiped $(SOURCE_DIR)/unpacked
cd $(SOURCE_DIR) && patch -s -N -p1 < ../../$(PATCH) || echo
- cd $(SOURCE_DIR) && autoconf && ./configure --prefix=$(PWD)
+ cd $(SOURCE_DIR) && autoconf && ./configure --prefix=`rospack find soqt`
cd $(SOURCE_DIR) && make && make install
touch installed
Modified: pkg/trunk/stacks/camera_drivers/libdc1394v2/Makefile
===================================================================
--- pkg/trunk/stacks/camera_drivers/libdc1394v2/Makefile 2009-08-04 01:38:17 UTC (rev 20605)
+++ pkg/trunk/stacks/camera_drivers/libdc1394v2/Makefile 2009-08-04 01:40:10 UTC (rev 20606)
@@ -8,7 +8,7 @@
installed: wiped $(SVN_DIR) patched
cd $(SVN_DIR) && autoreconf -i -s
- cd $(SVN_DIR) && CFLAGS="-g -O2 -fPIC" ./configure --prefix=$(PWD)/libdc1394v2 --enable-shared=no
+ cd $(SVN_DIR) && CFLAGS="-g -O2 -fPIC" ./configure --prefix=`rospack find libdc1394v2`/libdc1394v2 --enable-shared=no
cd $(SVN_DIR) && ln -fs /usr/bin/libtool
cd $(SVN_DIR) && make
cd $(SVN_DIR) && make install
Modified: pkg/trunk/stacks/common/bfl/Makefile
===================================================================
--- pkg/trunk/stacks/common/bfl/Makefile 2009-08-04 01:38:17 UTC (rev 20605)
+++ pkg/trunk/stacks/common/bfl/Makefile 2009-08-04 01:40:10 UTC (rev 20606)
@@ -7,7 +7,7 @@
INSTALL_DIR = bfl-boost
CMAKE = cmake
BOOST_INCLUDE =$(shell rosboost-cfg --include_dirs)
-CMAKE_ARGS = -DCMAKE_INSTALL_PREFIX=$(PWD)/$(INSTALL_DIR)/ \
+CMAKE_ARGS = -DCMAKE_INSTALL_PREFIX=`rospack find bfl`/$(INSTALL_DIR)/ \
-DCMAKE_INCLUDE_PATH=$(BOOST_INCLUDE) -DMATRIX_LIB=boost -DRNG_LIB=boost
include $(shell rospack find mk)/svn_checkout.mk
Modified: pkg/trunk/stacks/laser_drivers/sicktoolbox/Makefile
===================================================================
--- pkg/trunk/stacks/laser_drivers/sicktoolbox/Makefile 2009-08-04 01:38:17 UTC (rev 20605)
+++ pkg/trunk/stacks/laser_drivers/sicktoolbox/Makefile 2009-08-04 01:40:10 UTC (rev 20606)
@@ -10,7 +10,7 @@
include $(shell rospack find mk)/download_unpack_build.mk
installed: wiped $(SOURCE_DIR)/unpacked
- cd $(SOURCE_DIR) && ./configure --prefix=$(PWD)/sicktoolbox
+ cd $(SOURCE_DIR) && ./configure --prefix=`rospack find sicktoolbox`/sicktoolbox
cd $(SOURCE_DIR) && make && make install
touch installed
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <tf...@us...> - 2009-08-04 01:49:46
|
Revision: 20607
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=20607&view=rev
Author: tfoote
Date: 2009-08-04 01:49:34 +0000 (Tue, 04 Aug 2009)
Log Message:
-----------
trying to make test displays more visible
Modified Paths:
--------------
pkg/trunk/demos/pr2_gazebo/prototype1.launch
pkg/trunk/demos/test_2dnav_gazebo/test_2dnav_empty_amcl_axis.launch
pkg/trunk/demos/test_2dnav_gazebo/test_2dnav_empty_axis.launch
pkg/trunk/demos/test_2dnav_gazebo/test_2dnav_empty_diagonal.launch
pkg/trunk/demos/test_2dnav_gazebo/test_2dnav_empty_odom.launch
pkg/trunk/demos/test_2dnav_gazebo/test_2dnav_empty_rotation.launch
pkg/trunk/demos/test_2dnav_gazebo/test_2dnav_wg.launch
pkg/trunk/stacks/simulators/gazebo/launch/empty_world.launch
Modified: pkg/trunk/demos/pr2_gazebo/prototype1.launch
===================================================================
--- pkg/trunk/demos/pr2_gazebo/prototype1.launch 2009-08-04 01:40:10 UTC (rev 20606)
+++ pkg/trunk/demos/pr2_gazebo/prototype1.launch 2009-08-04 01:49:34 UTC (rev 20607)
@@ -4,7 +4,7 @@
<param name="robot_description" command="$(find xacro)/xacro.py '$(find pr2_defs)/robots/prototype1.xacro.xml'" />
<!-- push robot_description to factory and spawn robot in gazebo -->
- <node pkg="gazebo_tools" type="urdf2factory" args="robot_description 0 0 0 0 0 0 prototype1_model" respawn="false" output="screen" />
+ <node pkg="gazebo_tools" type="urdf2factory" args="robot_description 0 0 0 0 0 0 prototype1_model" respawn="false" />
<!-- Joystick uncomment if you have joystick
<param name="joy/deadzone" value="5000"/>
Modified: pkg/trunk/demos/test_2dnav_gazebo/test_2dnav_empty_amcl_axis.launch
===================================================================
--- pkg/trunk/demos/test_2dnav_gazebo/test_2dnav_empty_amcl_axis.launch 2009-08-04 01:40:10 UTC (rev 20606)
+++ pkg/trunk/demos/test_2dnav_gazebo/test_2dnav_empty_amcl_axis.launch 2009-08-04 01:49:34 UTC (rev 20607)
@@ -7,16 +7,16 @@
<include file="$(find pr2_gazebo)/prototype1.launch"/>
<!-- load map -->
- <node pkg="map_server" type="map_server" args="$(find gazebo_worlds)/Media/materials/textures/map_blank.png 0.1" respawn="false" output="screen" />
+ <node pkg="map_server" type="map_server" args="$(find gazebo_worlds)/Media/materials/textures/map_blank.png 0.1" respawn="false" />
<!-- nav-stack -->
<include file="$(find 2dnav_gazebo)/2dnav-stack-amcl.launch"/>
<!-- for visualization -->
<!--
- <node pkg="nav_view" type="nav_view" respawn="false" output="screen" />
- <node pkg="rviz" type="rviz" respawn="false" output="screen" />
- <node pkg="rosviz" type="rxplot" respawn="false" output="screen" args="-m. -p10 /state/pos/x,/state/pos/y,/state/pos/th" />
+ <node pkg="nav_view" type="nav_view" respawn="false" />
+ <node pkg="rviz" type="rviz" respawn="false" />
+ <node pkg="rosviz" type="rxplot" respawn="false" args="-m. -p10 /state/pos/x,/state/pos/y,/state/pos/th" />
-->
<!--
Modified: pkg/trunk/demos/test_2dnav_gazebo/test_2dnav_empty_axis.launch
===================================================================
--- pkg/trunk/demos/test_2dnav_gazebo/test_2dnav_empty_axis.launch 2009-08-04 01:40:10 UTC (rev 20606)
+++ pkg/trunk/demos/test_2dnav_gazebo/test_2dnav_empty_axis.launch 2009-08-04 01:49:34 UTC (rev 20607)
@@ -8,16 +8,16 @@
<include file="$(find pr2_gazebo)/prototype1.launch"/>
<!-- load map -->
- <node pkg="map_server" type="map_server" args="$(find gazebo_worlds)/Media/materials/textures/map_blank.png 0.1" respawn="false" output="screen" />
+ <node pkg="map_server" type="map_server" args="$(find gazebo_worlds)/Media/materials/textures/map_blank.png 0.1" respawn="false" />
<!-- nav-stack -->
<include file="$(find 2dnav_gazebo)/2dnav-stack-fake_localization.launch"/>
<!-- for visualization -->
<!--
- <node pkg="nav_view" type="nav_view" respawn="false" output="screen" />
- <node pkg="rviz" type="rviz" respawn="false" output="screen" />
- <node pkg="rosviz" type="rxplot" respawn="false" output="screen" args="-m. -p10 /state/pos/x,/state/pos/y,/state/pos/th" />
+ <node pkg="nav_view" type="nav_view" respawn="false" />
+ <node pkg="rviz" type="rviz" respawn="false" />
+ <node pkg="rosviz" type="rxplot" respawn="false" args="-m. -p10 /state/pos/x,/state/pos/y,/state/pos/th" />
-->
<!--
Modified: pkg/trunk/demos/test_2dnav_gazebo/test_2dnav_empty_diagonal.launch
===================================================================
--- pkg/trunk/demos/test_2dnav_gazebo/test_2dnav_empty_diagonal.launch 2009-08-04 01:40:10 UTC (rev 20606)
+++ pkg/trunk/demos/test_2dnav_gazebo/test_2dnav_empty_diagonal.launch 2009-08-04 01:49:34 UTC (rev 20607)
@@ -8,15 +8,15 @@
<include file="$(find pr2_gazebo)/prototype1.launch"/>
<!-- load map -->
- <node pkg="map_server" type="map_server" args="$(find gazebo_worlds)/Media/materials/textures/map_blank.png 0.1" respawn="false" output="screen" />
+ <node pkg="map_server" type="map_server" args="$(find gazebo_worlds)/Media/materials/textures/map_blank.png 0.1" respawn="false" />
<!-- nav-stack -->
<include file="$(find 2dnav_gazebo)/2dnav-stack-fake_localization.launch"/>
<!-- for visualization -->
<!--
- <node pkg="rviz" type="rviz" respawn="false" output="screen" />
- <node pkg="nav_view" type="nav_view" respawn="false" output="screen" />
+ <node pkg="rviz" type="rviz" respawn="false" />
+ <node pkg="nav_view" type="nav_view" respawn="false" />
-->
<!--
Modified: pkg/trunk/demos/test_2dnav_gazebo/test_2dnav_empty_odom.launch
===================================================================
--- pkg/trunk/demos/test_2dnav_gazebo/test_2dnav_empty_odom.launch 2009-08-04 01:40:10 UTC (rev 20606)
+++ pkg/trunk/demos/test_2dnav_gazebo/test_2dnav_empty_odom.launch 2009-08-04 01:49:34 UTC (rev 20607)
@@ -8,15 +8,15 @@
<include file="$(find pr2_gazebo)/prototype1.launch"/>
<!-- load map -->
- <node pkg="map_server" type="map_server" args="$(find gazebo_worlds)/Media/materials/textures/map_blank.png 0.1" respawn="false" output="screen" />
+ <node pkg="map_server" type="map_server" args="$(find gazebo_worlds)/Media/materials/textures/map_blank.png 0.1" respawn="false" />
<!-- nav-stack -->
<include file="$(find 2dnav_gazebo)/2dnav-stack-fake_localization.launch"/>
<!-- for visualization -->
<!--
- <node pkg="nav_view" type="nav_view" respawn="false" output="screen" />
- <node pkg="rviz" type="rviz" respawn="false" output="screen" />
+ <node pkg="nav_view" type="nav_view" respawn="false" />
+ <node pkg="rviz" type="rviz" respawn="false" />
-->
<!--
Modified: pkg/trunk/demos/test_2dnav_gazebo/test_2dnav_empty_rotation.launch
===================================================================
--- pkg/trunk/demos/test_2dnav_gazebo/test_2dnav_empty_rotation.launch 2009-08-04 01:40:10 UTC (rev 20606)
+++ pkg/trunk/demos/test_2dnav_gazebo/test_2dnav_empty_rotation.launch 2009-08-04 01:49:34 UTC (rev 20607)
@@ -8,15 +8,15 @@
<include file="$(find pr2_gazebo)/prototype1.launch"/>
<!-- load map -->
- <node pkg="map_server" type="map_server" args="$(find gazebo_worlds)/Media/materials/textures/map_blank.png 0.1" respawn="false" output="screen" />
+ <node pkg="map_server" type="map_server" args="$(find gazebo_worlds)/Media/materials/textures/map_blank.png 0.1" respawn="false" />
<!-- nav-stack -->
<include file="$(find 2dnav_gazebo)/2dnav-stack-fake_localization.launch"/>
<!-- for visualization -->
<!--
- <node pkg="rviz" type="rviz" respawn="false" output="screen" />
- <node pkg="nav_view" type="nav_view" respawn="false" output="screen" />
+ <node pkg="rviz" type="rviz" respawn="false" />
+ <node pkg="nav_view" type="nav_view" respawn="false" />
-->
<!--
Modified: pkg/trunk/demos/test_2dnav_gazebo/test_2dnav_wg.launch
===================================================================
--- pkg/trunk/demos/test_2dnav_gazebo/test_2dnav_wg.launch 2009-08-04 01:40:10 UTC (rev 20606)
+++ pkg/trunk/demos/test_2dnav_gazebo/test_2dnav_wg.launch 2009-08-04 01:49:34 UTC (rev 20607)
@@ -8,16 +8,16 @@
<include file="$(find pr2_gazebo)/prototype1.launch"/>
<!-- load map -->
- <node pkg="map_server" type="map_server" args="$(find gazebo_worlds)/Media/materials/textures/willowMap25mm.png 0.025" respawn="false" output="screen" />
+ <node pkg="map_server" type="map_server" args="$(find gazebo_worlds)/Media/materials/textures/willowMap25mm.png 0.025" respawn="false" />
<!--
- <node pkg="map_server" type="map_server" args="$(find gazebo_worlds)/Media/materials/textures/willowMap.png 0.1" respawn="false" output="screen" />
+ <node pkg="map_server" type="map_server" args="$(find gazebo_worlds)/Media/materials/textures/willowMap.png 0.1" respawn="false" />
-->
<!-- nav-stack -->
<include file="$(find 2dnav_gazebo)/2dnav-stack-fake_localization.launch"/>
<include file="$(find 2dnav_pr2)/rviz/rviz_move_base.launch"/>
- <node pkg="rxtools" type="rxconsole" respawn="false" output="screen" />
+ <node pkg="rxtools" type="rxconsole" respawn="false" />
<!--
<param name="/trex/ping_frequency" value="1"/>
<node pkg="highlevel_controllers" type="recharge_controller" args="" respawn="false" />
Modified: pkg/trunk/stacks/simulators/gazebo/launch/empty_world.launch
===================================================================
--- pkg/trunk/stacks/simulators/gazebo/launch/empty_world.launch 2009-08-04 01:40:10 UTC (rev 20606)
+++ pkg/trunk/stacks/simulators/gazebo/launch/empty_world.launch 2009-08-04 01:49:34 UTC (rev 20607)
@@ -4,10 +4,10 @@
<param name="/use_sim_time" value="true" />
<!--
- <node pkg="gazebo" launch-prefix="glc-capture" type="gazebo" args="$(find gazebo_worlds)/worlds/empty.world" respawn="false" output="screen">
- <node pkg="gazebo" launch-prefix="gdb - -args" type="gazebo" args="$(find gazebo_worlds)/worlds/empty.world" respawn="false" output="screen">
+ <node pkg="gazebo" launch-prefix="glc-capture" type="gazebo" args="$(find gazebo_worlds)/worlds/empty.world" respawn="false" >
+ <node pkg="gazebo" launch-prefix="gdb - -args" type="gazebo" args="$(find gazebo_worlds)/worlds/empty.world" respawn="false" >
-->
- <node pkg="gazebo" type="gazebo" args="$(find gazebo_worlds)/worlds/empty.world" respawn="false" output="screen">
+ <node pkg="gazebo" type="gazebo" args="$(find gazebo_worlds)/worlds/empty.world" respawn="false" >
<env name="LD_LIBRARY_PATH" value="$(find gazebo_plugin)/lib:$(find gazebo)/gazebo/lib:$(find Cg)/Cg/lib:$(optenv LD_LIBRARY_PATH)" />
<env name="GAZEBO_RESOURCE_PATH" value="$(find pr2_ogre):$(find gazebo_worlds):$(find gazebo)/gazebo/share/gazebo" />
<env name="OGRE_RESOURCE_PATH" value="$(find ogre)/ogre/lib/OGRE" />
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <ei...@us...> - 2009-08-04 03:19:37
|
Revision: 20621
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=20621&view=rev
Author: eitanme
Date: 2009-08-04 03:19:26 +0000 (Tue, 04 Aug 2009)
Log Message:
-----------
Changes to costmap_2d as per the api review. It is now api cleared.
Modified Paths:
--------------
pkg/trunk/applications/2dnav_pr2_app/2dnav_pr2/config/costmap_common_params.yaml
pkg/trunk/demos/2dnav_erratic/config/costmap_common_params.yaml
pkg/trunk/demos/door_demos/launch/move_base_door.launch
pkg/trunk/demos/erratic_gazebo/costmap_common_params.yaml
pkg/trunk/highlevel/doors/doors_core/src/move_base_door_action.cpp
pkg/trunk/highlevel/move_base_stage/config/costmap_common_params.yaml
pkg/trunk/motion_planning/mpglue/src/costmap.cpp
pkg/trunk/motion_planning/mpglue/src/costmapper.cpp
pkg/trunk/motion_planning/planning_research/sbpl_door_planner_action/sbpl_door_action_planner.launch
pkg/trunk/nav/nav_apps/costmap_2d_tutorials/launch_example/costmap_params.yaml
pkg/trunk/nav/nav_apps/costmap_2d_tutorials/src/costmap_test.cpp
pkg/trunk/nav/people_aware_nav/src/move_base_constrained.cpp
pkg/trunk/sandbox/teleop_anti_collision/src/anti_collision_base_controller.cpp
pkg/trunk/stacks/navigation/base_local_planner/include/base_local_planner/trajectory_planner_ros.h
pkg/trunk/stacks/navigation/base_local_planner/src/trajectory_planner.cpp
pkg/trunk/stacks/navigation/base_local_planner/src/trajectory_planner_ros.cpp
pkg/trunk/stacks/navigation/carrot_planner/src/carrot_planner.cpp
pkg/trunk/stacks/navigation/costmap_2d/include/costmap_2d/costmap_2d.h
pkg/trunk/stacks/navigation/costmap_2d/include/costmap_2d/costmap_2d_ros.h
pkg/trunk/stacks/navigation/costmap_2d/include/costmap_2d/observation.h
pkg/trunk/stacks/navigation/costmap_2d/include/costmap_2d/observation_buffer.h
pkg/trunk/stacks/navigation/costmap_2d/launch_costmap_2d_ros.xml
pkg/trunk/stacks/navigation/costmap_2d/mainpage.dox
pkg/trunk/stacks/navigation/costmap_2d/manifest.xml
pkg/trunk/stacks/navigation/costmap_2d/src/costmap_2d.cpp
pkg/trunk/stacks/navigation/costmap_2d/src/costmap_2d_publisher.cpp
pkg/trunk/stacks/navigation/costmap_2d/src/costmap_2d_ros.cpp
pkg/trunk/stacks/navigation/costmap_2d/src/observation_buffer.cpp
pkg/trunk/stacks/navigation/costmap_2d/src/voxel_costmap_2d.cpp
pkg/trunk/stacks/navigation/move_base/src/move_base.cpp
pkg/trunk/stacks/navigation/move_base/src/move_base_old.cpp
pkg/trunk/stacks/navigation/navfn/src/navfn_node.cpp
pkg/trunk/stacks/navigation/navfn/src/navfn_ros.cpp
Modified: pkg/trunk/applications/2dnav_pr2_app/2dnav_pr2/config/costmap_common_params.yaml
===================================================================
--- pkg/trunk/applications/2dnav_pr2_app/2dnav_pr2/config/costmap_common_params.yaml 2009-08-04 03:18:55 UTC (rev 20620)
+++ pkg/trunk/applications/2dnav_pr2_app/2dnav_pr2/config/costmap_common_params.yaml 2009-08-04 03:19:26 UTC (rev 20621)
@@ -16,7 +16,7 @@
lethal_cost_threshold: 100
# BEGIN VOXEL STUFF
-observation_topics: base_scan_marking base_scan tilt_scan ground_object_cloud
+observation_sources: base_scan_marking base_scan tilt_scan ground_object_cloud
base_scan_marking: {sensor_frame: base_laser, data_type: PointCloud, expected_update_rate: 0.2,
observation_persistence: 0.0, marking: true, clearing: false, min_obstacle_height: 0.08, max_obstacle_height: 2.0}
Modified: pkg/trunk/demos/2dnav_erratic/config/costmap_common_params.yaml
===================================================================
--- pkg/trunk/demos/2dnav_erratic/config/costmap_common_params.yaml 2009-08-04 03:18:55 UTC (rev 20620)
+++ pkg/trunk/demos/2dnav_erratic/config/costmap_common_params.yaml 2009-08-04 03:19:26 UTC (rev 20621)
@@ -15,6 +15,6 @@
inflation_radius: 0.55
cost_scaling_factor: 10.0
lethal_cost_threshold: 100
-observation_topics: scan
-scan: {sensor_frame: frame_from_message, data_type: LaserScan, expected_update_rate: 0.2,
+observation_sources: scan
+scan: {data_type: LaserScan, expected_update_rate: 0.2,
observation_persistence: 0.0, marking: true, clearing: true, max_obstacle_height: 0.4, min_obstacle_height: 0.08}
Modified: pkg/trunk/demos/door_demos/launch/move_base_door.launch
===================================================================
--- pkg/trunk/demos/door_demos/launch/move_base_door.launch 2009-08-04 03:18:55 UTC (rev 20620)
+++ pkg/trunk/demos/door_demos/launch/move_base_door.launch 2009-08-04 03:19:26 UTC (rev 20621)
@@ -31,24 +31,21 @@
<param name="costmap/max_obstacle_height" value="2.0" />
<param name="costmap/raytrace_range" value="1.5" />
- <!--param name="costmap/observation_topics" value="base_scan base_scan_marking" /-->
- <param name="costmap/observation_topics" value="base_scan base_scan_marking" />
+ <!--param name="costmap/observation_sources" value="base_scan base_scan_marking" /-->
+ <param name="costmap/observation_sources" value="base_scan base_scan_marking" />
- <param name="costmap/base_scan/sensor_frame" value="frame_from_message" />
<param name="costmap/base_scan/observation_persistence" value="0.0" />
<param name="costmap/base_scan/expected_update_rate" value="0.2" />
<param name="costmap/base_scan/data_type" value="LaserScan" />
<param name="costmap/base_scan/clearing" value="true" />
<param name="costmap/base_scan/marking" value="false" />
- <param name="costmap/base_scan_marking/sensor_frame" value="frame_from_message" />
<param name="costmap/base_scan_marking/observation_persistence" value="0.0" />
<param name="costmap/base_scan_marking/expected_update_rate" value="0.2" />
<param name="costmap/base_scan_marking/data_type" value="PointCloud" />
<param name="costmap/base_scan_marking/clearing" value="false" />
<param name="costmap/base_scan_marking/marking" value="true" />
- <param name="costmap/tilt_scan/sensor_frame" value="frame_from_message" />
<param name="costmap/tilt_scan/observation_persistence" value="0.0" />
<param name="costmap/tilt_scan/expected_update_rate" value="0.2" />
<param name="costmap/tilt_scan/data_type" value="LaserScan" />
Modified: pkg/trunk/demos/erratic_gazebo/costmap_common_params.yaml
===================================================================
--- pkg/trunk/demos/erratic_gazebo/costmap_common_params.yaml 2009-08-04 03:18:55 UTC (rev 20620)
+++ pkg/trunk/demos/erratic_gazebo/costmap_common_params.yaml 2009-08-04 03:19:26 UTC (rev 20621)
@@ -9,7 +9,7 @@
cost_scaling_factor: 25.0
lethal_cost_threshold: 100
- observation_topics: base_scan_marking base_scan
+ observation_sources: base_scan_marking base_scan
base_scan_marking: {sensor_frame: base_laser, data_type: PointCloud, expected_update_rate: 0.2,
observation_persistence: 0.0, marking: true, clearing: false, min_obstacle_height: 0.08, max_obstalce_height: 2.0}
Modified: pkg/trunk/highlevel/doors/doors_core/src/move_base_door_action.cpp
===================================================================
--- pkg/trunk/highlevel/doors/doors_core/src/move_base_door_action.cpp 2009-08-04 03:18:55 UTC (rev 20620)
+++ pkg/trunk/highlevel/doors/doors_core/src/move_base_door_action.cpp 2009-08-04 03:19:26 UTC (rev 20621)
@@ -98,7 +98,7 @@
//initialize the door opening planner
planner_ = new DoorReactivePlanner(ros_node_, tf_,&planner_cost_map_,control_frame_,global_frame_);
- ROS_INFO("MAP SIZE: %d, %d", planner_cost_map_.cellSizeX(), planner_cost_map_.cellSizeY());
+ ROS_INFO("MAP SIZE: %d, %d", planner_cost_map_.getSizeInCellsX(), planner_cost_map_.getSizeInCellsY());
ros_node_.advertise<manipulation_msgs::JointTraj>(control_topic_name_, 1);
last_diagnostics_publish_time_ = ros::Time::now();
Modified: pkg/trunk/highlevel/move_base_stage/config/costmap_common_params.yaml
===================================================================
--- pkg/trunk/highlevel/move_base_stage/config/costmap_common_params.yaml 2009-08-04 03:18:55 UTC (rev 20620)
+++ pkg/trunk/highlevel/move_base_stage/config/costmap_common_params.yaml 2009-08-04 03:19:26 UTC (rev 20621)
@@ -15,6 +15,6 @@
inflation_radius: 0.55
cost_scaling_factor: 10.0
lethal_cost_threshold: 100
-observation_topics: base_scan
-base_scan: {sensor_frame: frame_from_message, data_type: LaserScan, expected_update_rate: 0.2,
+observation_sources: base_scan
+base_scan: {data_type: LaserScan, expected_update_rate: 0.2,
observation_persistence: 0.0, marking: true, clearing: true, max_obstacle_height: 0.4, min_obstacle_height: 0.08}
Modified: pkg/trunk/motion_planning/mpglue/src/costmap.cpp
===================================================================
--- pkg/trunk/motion_planning/mpglue/src/costmap.cpp 2009-08-04 03:18:55 UTC (rev 20620)
+++ pkg/trunk/motion_planning/mpglue/src/costmap.cpp 2009-08-04 03:19:26 UTC (rev 20621)
@@ -59,14 +59,14 @@
{ return (*get_costmap_)()->getCircumscribedCost(); }
virtual ssize_t getXBegin() const { return 0; }
- virtual ssize_t getXEnd() const { return (*get_costmap_)()->cellSizeX(); }
+ virtual ssize_t getXEnd() const { return (*get_costmap_)()->getSizeInCellsX(); }
virtual ssize_t getYBegin() const { return 0; }
- virtual ssize_t getYEnd() const { return (*get_costmap_)()->cellSizeY(); }
+ virtual ssize_t getYEnd() const { return (*get_costmap_)()->getSizeInCellsY(); }
virtual bool isValidIndex(ssize_t index_x, ssize_t index_y) const {
costmap_2d::Costmap2D const * cm((*get_costmap_)());
- return (index_x >= 0) && (static_cast<size_t>(index_x) < cm->cellSizeX())
- && (index_y >= 0) && (static_cast<size_t>(index_y) < cm->cellSizeY()); }
+ return (index_x >= 0) && (static_cast<size_t>(index_x) < cm->getSizeInCellsX())
+ && (index_y >= 0) && (static_cast<size_t>(index_y) < cm->getSizeInCellsY()); }
virtual bool isLethal(ssize_t index_x, ssize_t index_y,
bool out_of_bounds_reply) const {
@@ -143,8 +143,8 @@
virtual void globalToLocal(double global_x, double global_y, double global_th,
double * local_x, double * local_y, double * local_th) const {
- *local_x = global_x - (*get_costmap_)()->originX();
- *local_y = global_y - (*get_costmap_)()->originY();
+ *local_x = global_x - (*get_costmap_)()->getOriginX();
+ *local_y = global_y - (*get_costmap_)()->getOriginY();
*local_th = global_th;
}
@@ -166,18 +166,18 @@
virtual void localToGlobal(double local_x, double local_y, double local_th,
double * global_x, double * global_y, double * global_th) const {
- *global_x = local_x + (*get_costmap_)()->originX();
- *global_y = local_y + (*get_costmap_)()->originY();
+ *global_x = local_x + (*get_costmap_)()->getOriginX();
+ *global_y = local_y + (*get_costmap_)()->getOriginY();
*global_th = local_th;
}
virtual void getOrigin(double * ox, double * oy, double * oth) const {
- *ox = (*get_costmap_)()->originX();
- *oy = (*get_costmap_)()->originY();
+ *ox = (*get_costmap_)()->getOriginX();
+ *oy = (*get_costmap_)()->getOriginY();
*oth = 0;
}
- virtual double getResolution() const { return (*get_costmap_)()->resolution(); }
+ virtual double getResolution() const { return (*get_costmap_)()->getResolution(); }
mpglue::costmap_2d_getter * get_costmap_;
};
Modified: pkg/trunk/motion_planning/mpglue/src/costmapper.cpp
===================================================================
--- pkg/trunk/motion_planning/mpglue/src/costmapper.cpp 2009-08-04 03:18:55 UTC (rev 20620)
+++ pkg/trunk/motion_planning/mpglue/src/costmapper.cpp 2009-08-04 03:19:26 UTC (rev 20621)
@@ -174,8 +174,8 @@
unsigned int bbx1(std::numeric_limits<unsigned int>::min());
unsigned int bby0(std::numeric_limits<unsigned int>::max());
unsigned int bby1(std::numeric_limits<unsigned int>::min());
- unsigned int const sizex(cm_->cellSizeX());
- unsigned int const sizey(cm_->cellSizeY());
+ unsigned int const sizex(cm_->getSizeInCellsX());
+ unsigned int const sizey(cm_->getSizeInCellsY());
if (removed_obstacle_indices) {
for (index_collection_t::const_iterator irem(removed_obstacle_indices->begin());
@@ -255,8 +255,8 @@
// wastes some operations, but if you want efficiency you
// probably want to use costmap_2d::Costmap2D directly anyway,
// without passing through mpglue.
- double const resolution(cm_->resolution());
- double const inflation2(2 * cm_->inflationRadius());
+ double const resolution(cm_->getResolution());
+ double const inflation2(2 * cm_->getInflationRadius());
double const wx(resolution * 0.5 * (bbx0 + bbx1));
double const wy(resolution * 0.5 * (bby0 + bby1));
double const w_size_x(resolution * (bbx1 - bbx0) + inflation2);
Modified: pkg/trunk/motion_planning/planning_research/sbpl_door_planner_action/sbpl_door_action_planner.launch
===================================================================
--- pkg/trunk/motion_planning/planning_research/sbpl_door_planner_action/sbpl_door_action_planner.launch 2009-08-04 03:18:55 UTC (rev 20620)
+++ pkg/trunk/motion_planning/planning_research/sbpl_door_planner_action/sbpl_door_action_planner.launch 2009-08-04 03:19:26 UTC (rev 20621)
@@ -15,9 +15,8 @@
<param name="costmap/max_obstacle_height" value="2.0" />
<param name="costmap/raytrace_range" value="3.0" />
- <param name="costmap/observation_topics" value="base_scan" />
+ <param name="costmap/observation_sources" value="base_scan" />
- <param name="costmap/base_scan/sensor_frame" value="frame_from_message" />
<param name="costmap/base_scan/observation_persistence" value="0.0" />
<param name="costmap/base_scan/expected_update_rate" value="0.2" />
<param name="costmap/base_scan/data_type" value="LaserScan" />
Modified: pkg/trunk/nav/nav_apps/costmap_2d_tutorials/launch_example/costmap_params.yaml
===================================================================
--- pkg/trunk/nav/nav_apps/costmap_2d_tutorials/launch_example/costmap_params.yaml 2009-08-04 03:18:55 UTC (rev 20620)
+++ pkg/trunk/nav/nav_apps/costmap_2d_tutorials/launch_example/costmap_params.yaml 2009-08-04 03:19:26 UTC (rev 20621)
@@ -42,20 +42,20 @@
circumscribed_radius: 0.46
inflation_radius: 0.55
-#The observation_topics list allows users of a Costmap2DROS object to specify what topics should be used
+#The observation_sources list allows users of a Costmap2DROS object to specify what topics should be used
#in conjunction with the underlying Costmap2D object. Topics should be listed here, separated by spaces and
#should be configured in their individual namespaces as shown below. Each topic in this list will be subscribed
#to and managed by the Costmap2DROS object subject to its configuration settings.
-observation_topics: base_scan
+observation_sources: base_scan
-#This is an example of topic configuration for the base_scan topic listed in the observation_topics list above.
+#This is an example of topic configuration for the base_scan topic listed in the observation_sources list above.
#Notice that the base scan receives its own namespace to read its configuration from.
base_scan:
#The sensor_frame parameter specifies what frame the origin of the sensor is
-#assumed to be in. The special value "frame_from_message" infers the origin
+#assumed to be in. Leaving this blank infers the origin
#frame from the frame_id sent in each message over the base_scan topic. Any
#other string will override the frame_id sent in the message.
- sensor_frame: frame_from_message
+ sensor_frame: base_laser
#The data_type parameter specifies what message type to expect for the topic. The
#two supported message types at this point are LaserScan and PointCloud.
Modified: pkg/trunk/nav/nav_apps/costmap_2d_tutorials/src/costmap_test.cpp
===================================================================
--- pkg/trunk/nav/nav_apps/costmap_2d_tutorials/src/costmap_test.cpp 2009-08-04 03:18:55 UTC (rev 20620)
+++ pkg/trunk/nav/nav_apps/costmap_2d_tutorials/src/costmap_test.cpp 2009-08-04 03:19:26 UTC (rev 20621)
@@ -44,7 +44,7 @@
costmap_ros.getCostmapCopy(costmap);
//do whatever you want with the map here
- ROS_INFO("The size of the map in meters is: (%.2f, %.2f)", costmap.metersSizeX(), costmap.metersSizeY());
+ ROS_INFO("The size of the map in meters is: (%.2f, %.2f)", costmap.getSizeInMetersX(), costmap.getSizeInMetersY());
}
int main(int argc, char** argv){
Modified: pkg/trunk/nav/people_aware_nav/src/move_base_constrained.cpp
===================================================================
--- pkg/trunk/nav/people_aware_nav/src/move_base_constrained.cpp 2009-08-04 03:18:55 UTC (rev 20620)
+++ pkg/trunk/nav/people_aware_nav/src/move_base_constrained.cpp 2009-08-04 03:19:26 UTC (rev 20621)
@@ -74,7 +74,7 @@
//initialize the NavFn planner
planner_ = new NavfnROS("NavfnROS", *planner_costmap_ros_);
- ROS_INFO("MAP SIZE: %d, %d", planner_costmap_.cellSizeX(), planner_costmap_.cellSizeY());
+ ROS_INFO("MAP SIZE: %d, %d", planner_costmap_.getSizeInCellsX(), planner_costmap_.getSizeInCellsY());
//create the ros wrapper for the controller's costmap... and initializer a pointer we'll use with the underlying map
controller_costmap_ros_ = new Costmap2DROS("local_costmap", tf_);
Modified: pkg/trunk/sandbox/teleop_anti_collision/src/anti_collision_base_controller.cpp
===================================================================
--- pkg/trunk/sandbox/teleop_anti_collision/src/anti_collision_base_controller.cpp 2009-08-04 03:18:55 UTC (rev 20620)
+++ pkg/trunk/sandbox/teleop_anti_collision/src/anti_collision_base_controller.cpp 2009-08-04 03:19:26 UTC (rev 20621)
@@ -50,12 +50,12 @@
costmap_ros_ = new costmap_2d::Costmap2DROS("", tf_);
//we'll get the parameters for the robot radius from the costmap we're associated with
- inscribed_radius_ = costmap_ros_->inscribedRadius();
- circumscribed_radius_ = costmap_ros_->circumscribedRadius();
- inflation_radius_ = costmap_ros_->inflationRadius();
- global_frame_ = costmap_ros_->globalFrame();
- robot_base_frame_ = costmap_ros_->baseFrame();
- footprint_spec_ = costmap_ros_->robotFootprint();
+ inscribed_radius_ = costmap_ros_->getInscribedRadius();
+ circumscribed_radius_ = costmap_ros_->getCircumscribedRadius();
+ inflation_radius_ = costmap_ros_->getInflationRadius();
+ global_frame_ = costmap_ros_->getGlobalFrameID();
+ robot_base_frame_ = costmap_ros_->getBaseFrameID();
+ footprint_spec_ = costmap_ros_->getRobotFootprint();
ros_node_.param("~controller_frequency", controller_frequency_, 10.0);
Modified: pkg/trunk/stacks/navigation/base_local_planner/include/base_local_planner/trajectory_planner_ros.h
===================================================================
--- pkg/trunk/stacks/navigation/base_local_planner/include/base_local_planner/trajectory_planner_ros.h 2009-08-04 03:18:55 UTC (rev 20620)
+++ pkg/trunk/stacks/navigation/base_local_planner/include/base_local_planner/trajectory_planner_ros.h 2009-08-04 03:19:26 UTC (rev 20621)
@@ -212,6 +212,7 @@
bool prune_plan_;
ros::Publisher footprint_pub_, g_plan_pub_, l_plan_pub_;
ros::Subscriber odom_sub_;
+ boost::recursive_mutex odom_lock_;
};
};
Modified: pkg/trunk/stacks/navigation/base_local_planner/src/trajectory_planner.cpp
===================================================================
--- pkg/trunk/stacks/navigation/base_local_planner/src/trajectory_planner.cpp 2009-08-04 03:18:55 UTC (rev 20620)
+++ pkg/trunk/stacks/navigation/base_local_planner/src/trajectory_planner.cpp 2009-08-04 03:19:26 UTC (rev 20621)
@@ -58,7 +58,7 @@
double backup_vel,
bool dwa, bool heading_scoring, double heading_scoring_timestep, bool simple_attractor,
vector<double> y_vels)
- : map_(costmap.cellSizeX(), costmap.cellSizeY()), costmap_(costmap),
+ : map_(costmap.getSizeInCellsX(), costmap.getSizeInCellsY()), costmap_(costmap),
world_model_(world_model), footprint_spec_(footprint_spec),
inscribed_radius_(inscribed_radius), circumscribed_radius_(circumscribed_radius),
goal_x_(0), goal_y_(0),
Modified: pkg/trunk/stacks/navigation/base_local_planner/src/trajectory_planner_ros.cpp
===================================================================
--- pkg/trunk/stacks/navigation/base_local_planner/src/trajectory_planner_ros.cpp 2009-08-04 03:18:55 UTC (rev 20620)
+++ pkg/trunk/stacks/navigation/base_local_planner/src/trajectory_planner_ros.cpp 2009-08-04 03:19:26 UTC (rev 20621)
@@ -70,8 +70,8 @@
g_plan_pub_ = ros_node_.advertise<visualization_msgs::Polyline>("~global_plan", 1);
l_plan_pub_ = ros_node_.advertise<visualization_msgs::Polyline>("~local_plan", 1);
- global_frame_ = costmap_ros_.globalFrame();
- robot_base_frame_ = costmap_ros_.baseFrame();
+ global_frame_ = costmap_ros_.getGlobalFrameID();
+ robot_base_frame_ = costmap_ros_.getBaseFrameID();
ros_node_.param("~prune_plan", prune_plan_, true);
ros_node_.param("~yaw_goal_tolerance", yaw_goal_tolerance_, 0.05);
@@ -86,9 +86,9 @@
odom_sub_ = global_node.subscribe<deprecated_msgs::RobotBase2DOdom>(odom_topic, 1, boost::bind(&TrajectoryPlannerROS::odomCallback, this, _1));
//we'll get the parameters for the robot radius from the costmap we're associated with
- inscribed_radius_ = costmap_ros_.inscribedRadius();
- circumscribed_radius_ = costmap_ros_.circumscribedRadius();
- inflation_radius_ = costmap_ros_.inflationRadius();
+ inscribed_radius_ = costmap_ros_.getInscribedRadius();
+ circumscribed_radius_ = costmap_ros_.getCircumscribedRadius();
+ inflation_radius_ = costmap_ros_.getInflationRadius();
ros_node_.param("~acc_lim_x", acc_lim_x, 2.5);
ros_node_.param("~acc_lim_y", acc_lim_y, 2.5);
@@ -127,7 +127,7 @@
ROS_ASSERT_MSG(world_model_type == "costmap", "At this time, only costmap world models are supported by this controller");
world_model_ = new CostmapModel(costmap_);
- tc_ = new TrajectoryPlanner(*world_model_, costmap_, costmap_ros_.robotFootprint(), inscribed_radius_, circumscribed_radius_,
+ tc_ = new TrajectoryPlanner(*world_model_, costmap_, costmap_ros_.getRobotFootprint(), inscribed_radius_, circumscribed_radius_,
acc_lim_x, acc_lim_y, acc_lim_theta, sim_time, sim_granularity, vx_samples, vtheta_samples, pdist_scale,
gdist_scale, occdist_scale, heading_lookahead, oscillation_reset_dist, escape_reset_dist, escape_reset_theta, holonomic_robot,
max_vel_x, min_vel_x, max_vel_th, min_vel_th, min_in_place_vel_th_, backup_vel,
@@ -147,6 +147,7 @@
}
bool TrajectoryPlannerROS::stopped(){
+ boost::recursive_mutex::scoped_lock(odom_lock_);
return abs(base_odom_.vel.th) <= rot_stopped_velocity_
&& abs(base_odom_.vel.x) <= trans_stopped_velocity_
&& abs(base_odom_.vel.y) <= trans_stopped_velocity_;
@@ -193,8 +194,7 @@
}
void TrajectoryPlannerROS::odomCallback(const deprecated_msgs::RobotBase2DOdom::ConstPtr& msg){
- base_odom_.lock();
-
+ boost::recursive_mutex::scoped_lock(odom_lock_);
try
{
tf::Stamped<btVector3> v_in(btVector3(msg->vel.x, msg->vel.y, 0), ros::Time(), msg->header.frame_id), v_out;
@@ -215,8 +215,6 @@
{
ROS_DEBUG("Extrapolation exception");
}
-
- base_odom_.unlock();
}
bool TrajectoryPlannerROS::goalReached(){
@@ -313,7 +311,7 @@
robot_msgs::PoseStamped newer_pose;
//we'll look ahead on the path the size of our window
- unsigned int needed_path_length = std::max(costmap_.cellSizeX(), costmap_.cellSizeY());
+ unsigned int needed_path_length = std::max(costmap_.getSizeInCellsX(), costmap_.getSizeInCellsY());
for(unsigned int i = 0; i < std::min((unsigned int)global_plan_.size(), needed_path_length); ++i){
const robot_msgs::PoseStamped& new_pose = global_plan_[i];
@@ -371,9 +369,12 @@
// Set current velocities from odometry
robot_msgs::PoseDot global_vel;
+
+ odom_lock_.lock();
global_vel.vel.vx = base_odom_.vel.x;
global_vel.vel.vy = base_odom_.vel.y;
global_vel.ang_vel.vz = base_odom_.vel.th;
+ odom_lock_.unlock();
tf::Stamped<tf::Pose> drive_cmds;
drive_cmds.frame_id_ = robot_base_frame_;
Modified: pkg/trunk/stacks/navigation/carrot_planner/src/carrot_planner.cpp
===================================================================
--- pkg/trunk/stacks/navigation/carrot_planner/src/carrot_planner.cpp 2009-08-04 03:18:55 UTC (rev 20620)
+++ pkg/trunk/stacks/navigation/carrot_planner/src/carrot_planner.cpp 2009-08-04 03:19:26 UTC (rev 20621)
@@ -42,14 +42,14 @@
CarrotPlanner::CarrotPlanner(std::string name, costmap_2d::Costmap2DROS& costmap_ros)
: costmap_ros_(costmap_ros){
ros::NodeHandle n(name);
- n.param("~step_size", step_size_, costmap_ros_.resolution());
+ n.param("~step_size", step_size_, costmap_ros_.getResolution());
n.param("~min_dist_from_robot", min_dist_from_robot_, 0.10);
costmap_ros_.getCostmapCopy(costmap_);
world_model_ = new base_local_planner::CostmapModel(costmap_);
//we'll get the parameters for the robot radius from the costmap we're associated with
- inscribed_radius_ = costmap_ros_.inscribedRadius();
- circumscribed_radius_ = costmap_ros_.circumscribedRadius();
- footprint_spec_ = costmap_ros_.robotFootprint();
+ inscribed_radius_ = costmap_ros_.getInscribedRadius();
+ circumscribed_radius_ = costmap_ros_.getCircumscribedRadius();
+ footprint_spec_ = costmap_ros_.getRobotFootprint();
}
@@ -91,9 +91,9 @@
plan.clear();
costmap_ros_.getCostmapCopy(costmap_);
- if(goal.header.frame_id != costmap_ros_.globalFrame()){
+ if(goal.header.frame_id != costmap_ros_.getGlobalFrameID()){
ROS_ERROR("This planner as configured will only accept goals in the %s frame, but a goal was sent in the %s frame.",
- costmap_ros_.globalFrame().c_str(), goal.header.frame_id.c_str());
+ costmap_ros_.getGlobalFrameID().c_str(), goal.header.frame_id.c_str());
return false;
}
Modified: pkg/trunk/stacks/navigation/costmap_2d/include/costmap_2d/costmap_2d.h
===================================================================
--- pkg/trunk/stacks/navigation/costmap_2d/include/costmap_2d/costmap_2d.h 2009-08-04 03:18:55 UTC (rev 20620)
+++ pkg/trunk/stacks/navigation/costmap_2d/include/costmap_2d/costmap_2d.h 2009-08-04 03:19:26 UTC (rev 20621)
@@ -208,13 +208,6 @@
}
/**
- * @brief Will return a copy of the underlying unsigned char array used as the costmap
- * <b>(NOTE: THE BURDEN IS ON THE USER TO DELETE THE ARRAY RETURNED)</b>
- * @return A copy of the underlying unsigned char array storing cost values
- */
- unsigned char* getCharMapCopy() const;
-
- /**
* @brief Will return a immutable pointer to the underlying unsigned char array used as the costmap
* @return A pointer to the underlying unsigned char array storing cost values
*/
@@ -224,73 +217,63 @@
* @brief Accessor for the x size of the costmap in cells
* @return The x size of the costmap
*/
- unsigned int cellSizeX() const;
+ unsigned int getSizeInCellsX() const;
/**
* @brief Accessor for the y size of the costmap in cells
* @return The y size of the costmap
*/
- unsigned int cellSizeY() const;
+ unsigned int getSizeInCellsY() const;
/**
* @brief Accessor for the x size of the costmap in meters
* @return The x size of the costmap (returns the centerpoint of the last legal cell in the map)
*/
- double metersSizeX() const;
+ double getSizeInMetersX() const;
/**
* @brief Accessor for the y size of the costmap in meters
* @return The y size of the costmap (returns the centerpoint of the last legal cell in the map)
*/
- double metersSizeY() const;
+ double getSizeInMetersY() const;
/**
* @brief Accessor for the x origin of the costmap
* @return The x origin of the costmap
*/
- double originX() const;
+ double getOriginX() const;
/**
* @brief Accessor for the y origin of the costmap
* @return The y origin of the costmap
*/
- double originY() const;
+ double getOriginY() const;
/**
* @brief Accessor for the resolution of the costmap
* @return The resolution of the costmap
*/
- double resolution() const;
+ double getResolution() const;
/**
* @brief Accessor for the inscribed radius of the robot
* @return The inscribed radius
*/
- double inscribedRadius() const { return inscribed_radius_; }
+ double getInscribedRadius() const { return inscribed_radius_; }
/**
* @brief Accessor for the circumscribed radius of the robot
* @return The circumscribed radius
*/
- double circumscribedRadius() const { return circumscribed_radius_; }
+ double getCircumscribedRadius() const { return circumscribed_radius_; }
/**
* @brief Accessor for the inflation radius of the robot
* @return The inflation radius
*/
- double inflationRadius() const { return inflation_radius_; }
+ double getInflationRadius() const { return inflation_radius_; }
/**
- * @brief Locks the costmap
- */
- void lock() { lock_.lock(); }
-
- /**
- * @brief Unlocks the costmap
- */
- void unlock() { lock_.unlock(); }
-
- /**
* @brief Sets the cost of a convex polygon to a desired value
* @param polygon The polygon to perform the operation on
* @param cost_value The value to set costs to
@@ -321,12 +304,13 @@
virtual void updateOrigin(double new_origin_x, double new_orig...
[truncated message content] |
|
From: <tf...@us...> - 2009-08-04 07:19:29
|
Revision: 20633
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=20633&view=rev
Author: tfoote
Date: 2009-08-04 07:19:09 +0000 (Tue, 04 Aug 2009)
Log Message:
-----------
merging in the changes to messages see ros-users email. THis is about half the common_msgs API changes
Modified Paths:
--------------
pkg/trunk/calibration/kinematic_calibration/deprecated/state_history.h
pkg/trunk/calibration/kinematic_calibration/manifest.xml
pkg/trunk/calibration/kinematic_calibration/msg/CalibrationData.msg
pkg/trunk/calibration/kinematic_calibration/msg/CalibrationData2.msg
pkg/trunk/calibration/kinematic_calibration/msg/RobotCal.msg
pkg/trunk/calibration/kinematic_calibration/msg/SensorKinematics.msg
pkg/trunk/calibration/kinematic_calibration/src/laser_head_grabber.cpp
pkg/trunk/calibration/kinematic_calibration/src/sensor_kinematics_grabber.cpp
pkg/trunk/calibration/kinematic_calibration/test/msg_cache_unittest.cpp
pkg/trunk/calibration/laser_cb_processing/src/laser_cb_processing.cpp
pkg/trunk/calibration/laser_cb_processing/src/pr2_tilt_laser_projector.cpp
pkg/trunk/calibration/mocap_msgs/manifest.xml
pkg/trunk/calibration/mocap_msgs/msg/MocapBody.msg
pkg/trunk/calibration/mocap_msgs/msg/MocapMarker.msg
pkg/trunk/calibration/wheel_odometry_calibration/src/odom_calib.cpp
pkg/trunk/calibration/wheel_odometry_calibration/src/odom_calib.h
pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/arm_trajectory_controller.h
pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_kinematics.h
pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_position_controller.h
pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/head_position_controller.h
pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/head_servoing_controller.h
pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/joint_trajectory_controller.h
pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/pr2_base_controller.h
pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/pr2_odometry.h
pkg/trunk/controllers/pr2_mechanism_controllers/manifest.xml
pkg/trunk/controllers/pr2_mechanism_controllers/msg/TrackLinkCmd.msg
pkg/trunk/controllers/pr2_mechanism_controllers/src/arm_trajectory_controller.cpp
pkg/trunk/controllers/pr2_mechanism_controllers/src/base_kinematics.cpp
pkg/trunk/controllers/pr2_mechanism_controllers/src/base_position_controller.cpp
pkg/trunk/controllers/pr2_mechanism_controllers/src/head_position_controller.cpp
pkg/trunk/controllers/pr2_mechanism_controllers/src/joint_trajectory_controller.cpp
pkg/trunk/controllers/pr2_mechanism_controllers/src/pr2_base_controller.cpp
pkg/trunk/controllers/pr2_mechanism_controllers/src/pr2_odometry.cpp
pkg/trunk/controllers/pr2_mechanism_controllers/srv/GraspPointSrv.srv
pkg/trunk/controllers/pr2_mechanism_controllers/test/test_odom.cpp
pkg/trunk/controllers/pr2_mechanism_controllers/test/test_run_base_controller.cpp
pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_pose_controller.h
pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_tff_controller.h
pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_trajectory_controller.h
pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_twist_controller.h
pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_twist_controller_ik.h
pkg/trunk/controllers/robot_mechanism_controllers/manifest.xml
pkg/trunk/controllers/robot_mechanism_controllers/msg/CartesianHybridState.msg
pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_hybrid_controller.cpp
pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_pose_controller.cpp
pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_tff_controller.cpp
pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_trajectory_controller.cpp
pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_twist_controller.cpp
pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_twist_controller_ik.cpp
pkg/trunk/demos/arm_gazebo/scripts/grasp_preprogrammed.py
pkg/trunk/demos/arm_gazebo/scripts/l_arm_default.py
pkg/trunk/demos/handhold/src/handhold.cpp
pkg/trunk/demos/milestone2/milestone2_tests/test/test_topological_map.cpp
pkg/trunk/demos/plug_in/src/servo.cpp
pkg/trunk/demos/plug_in_gazebo/scripts/set_plug.py
pkg/trunk/demos/plug_in_gazebo/scripts/set_plug_on_base.py
pkg/trunk/demos/tabletop_manipulation/tabletop_msgs/manifest.xml
pkg/trunk/demos/tabletop_manipulation/tabletop_msgs/msg/ObjectOnTable.msg
pkg/trunk/demos/tabletop_manipulation/tabletop_msgs/msg/Table.msg
pkg/trunk/demos/tabletop_manipulation/tabletop_srvs/manifest.xml
pkg/trunk/demos/test_2dnav_gazebo/set_goal.py
pkg/trunk/deprecated/deprecated_srvs/manifest.xml
pkg/trunk/deprecated/deprecated_srvs/srv/MoveToPose.srv
pkg/trunk/deprecated/deprecated_srvs/srv/PolledStereoCloud.srv
pkg/trunk/drivers/laser/laser_scan_annotator/manifest.xml
pkg/trunk/drivers/laser/laser_scan_annotator/msg/LaserScanAnnotated.msg
pkg/trunk/drivers/robot/pr2/pr2_phase_space/src/phase_space_fake_localization.cpp
pkg/trunk/drivers/robot/pr2/pr2_phase_space/src/phase_space_odometry.cpp
pkg/trunk/drivers/robot/pr2/pr2_phase_space/src/phase_space_pan_tilt.cpp
pkg/trunk/drivers/robot/pr2/pr2_phase_space/src/phase_space_pose_stamped.cpp
pkg/trunk/drivers/robot/pr2/pr2_phase_space/src/phase_space_tongs.cpp
pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/ros_block_laser.h
pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/ros_bumper.h
pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/ros_f3d.h
pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/ros_p3d.h
pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/ros_sim_iface.h
pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_block_laser.cpp
pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_bumper.cpp
pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_diffdrive.cpp
pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_f3d.cpp
pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_p3d.cpp
pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_sim_iface.cpp
pkg/trunk/highlevel/doors/door_functions/include/door_functions/door_functions.h
pkg/trunk/highlevel/doors/door_functions/src/door_functions.cpp
pkg/trunk/highlevel/doors/door_msgs/manifest.xml
pkg/trunk/highlevel/doors/door_msgs/msg/Door.msg
pkg/trunk/highlevel/doors/doors_core/include/doors_core/action_check_path.h
pkg/trunk/highlevel/doors/doors_core/include/doors_core/action_push_door.h
pkg/trunk/highlevel/doors/doors_core/include/doors_core/action_release_handle.h
pkg/trunk/highlevel/doors/doors_core/include/doors_core/action_unlatch_handle.h
pkg/trunk/highlevel/doors/doors_core/include/doors_core/door_reactive_planner.h
pkg/trunk/highlevel/doors/doors_core/include/doors_core/move_base_door_action.h
pkg/trunk/highlevel/doors/doors_core/src/action_check_path.cpp
pkg/trunk/highlevel/doors/doors_core/src/action_detect_handle.cpp
pkg/trunk/highlevel/doors/doors_core/src/action_detect_handle_no_camera.cpp
pkg/trunk/highlevel/doors/doors_core/src/action_open_door.cpp
pkg/trunk/highlevel/doors/doors_core/src/action_push_door.cpp
pkg/trunk/highlevel/doors/doors_core/src/action_runner.cpp
pkg/trunk/highlevel/doors/doors_core/src/action_unlatch_handle.cpp
pkg/trunk/highlevel/doors/doors_core/src/door_reactive_planner.cpp
pkg/trunk/highlevel/doors/doors_core/src/move_base_door_action.cpp
pkg/trunk/highlevel/doors/doors_core/test/test_executive.cpp
pkg/trunk/highlevel/doors/doors_core/test/test_planner.cpp
pkg/trunk/highlevel/doors/doors_core/test/trigger_check_path.cpp
pkg/trunk/highlevel/move_arm/include/move_arm/move_arm.h
pkg/trunk/highlevel/move_arm/src/move_arm_action.cpp
pkg/trunk/highlevel/plugs/plugs_core/include/plugs_core/action_detect_outlet_coarse.h
pkg/trunk/highlevel/plugs/plugs_core/include/plugs_core/action_detect_outlet_fine.h
pkg/trunk/highlevel/plugs/plugs_core/include/plugs_core/action_localize_plug_in_gripper.h
pkg/trunk/highlevel/plugs/plugs_core/include/plugs_core/action_plug_in.h
pkg/trunk/highlevel/plugs/plugs_core/include/plugs_core/action_plug_in2.h
pkg/trunk/highlevel/plugs/plugs_core/include/plugs_core/action_unplug.h
pkg/trunk/highlevel/plugs/plugs_core/manifest.xml
pkg/trunk/highlevel/plugs/plugs_core/msg/PlugInState.msg
pkg/trunk/highlevel/plugs/plugs_core/src/action_detect_outlet_coarse.cpp
pkg/trunk/highlevel/plugs/plugs_core/src/action_detect_outlet_fine.cpp
pkg/trunk/highlevel/plugs/plugs_core/src/action_plug_in.cpp
pkg/trunk/highlevel/plugs/plugs_core/src/action_plug_in2.cpp
pkg/trunk/highlevel/plugs/plugs_core/src/action_unplug.cpp
pkg/trunk/highlevel/plugs/plugs_core/src/run_detect_outlet_coarse.cpp
pkg/trunk/highlevel/plugs/plugs_core/src/run_detect_outlet_fine.cpp
pkg/trunk/highlevel/plugs/plugs_core/test/test_approach.cpp
pkg/trunk/highlevel/plugs/plugs_core/test/test_executive.cpp
pkg/trunk/highlevel/plugs/plugs_core/test/test_executive_hybrid.cpp
pkg/trunk/highlevel/plugs/plugs_core/test/test_executive_outlet_coarse.cpp
pkg/trunk/highlevel/plugs/plugs_core/test/test_plug_in.cpp
pkg/trunk/highlevel/plugs/plugs_core/test/test_stage_stow.cpp
pkg/trunk/highlevel/plugs/plugs_functions/include/plugs_functions/plugs_functions.h
pkg/trunk/highlevel/plugs/plugs_functions/src/plugs_functions.cpp
pkg/trunk/highlevel/plugs/plugs_msgs/manifest.xml
pkg/trunk/highlevel/plugs/plugs_msgs/msg/PlugStow.msg
pkg/trunk/highlevel/writing/writing_core/src/whiteboard_detector.cpp
pkg/trunk/mapping/annotated_map_msgs/manifest.xml
pkg/trunk/mapping/annotated_map_msgs/msg/TaggedPolygon3D.msg
pkg/trunk/mapping/annotated_planar_patch_map/include/annotated_planar_patch_map/annotated_map_lib.h
pkg/trunk/mapping/annotated_planar_patch_map/include/annotated_planar_patch_map/map_base_assembler_srv.h
pkg/trunk/mapping/annotated_planar_patch_map/include/annotated_planar_patch_map/projection.h
pkg/trunk/mapping/annotated_planar_patch_map/manifest.xml
pkg/trunk/mapping/annotated_planar_patch_map/src/annotated_map_bind.cpp
pkg/trunk/mapping/annotated_planar_patch_map/src/annotated_map_lib.cpp
pkg/trunk/mapping/annotated_planar_patch_map/src/annotated_map_query_snapshotter.cpp
pkg/trunk/mapping/annotated_planar_patch_map/src/annotated_map_snapshotter.cpp
pkg/trunk/mapping/annotated_planar_patch_map/src/annotation2d_lifter_to_pcd_via_service.cpp
pkg/trunk/mapping/annotated_planar_patch_map/src/binding.cpp
pkg/trunk/mapping/annotated_planar_patch_map/src/pcd_assembler_srv.cpp
pkg/trunk/mapping/annotated_planar_patch_map/src/projection.cpp
pkg/trunk/mapping/annotated_planar_patch_map/test/projection_test.cpp
pkg/trunk/mapping/collision_map/src/collision_map.cpp
pkg/trunk/mapping/collision_map/src/collision_map_buffer.cpp
pkg/trunk/mapping/collision_map/src/collision_map_occ.cpp
pkg/trunk/mapping/collision_map/src/collision_map_self_occ.cpp
pkg/trunk/mapping/door_stereo_detector/src/door_stereo_detector.cpp
pkg/trunk/mapping/door_tracker/manifest.xml
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/mapping/planar_patch_map/manifest.xml
pkg/trunk/mapping/planar_patch_map/src/planar_patch_map.cpp
pkg/trunk/mapping/planar_patch_map/src/planar_patch_map_omp.cpp
pkg/trunk/mapping/stereo_convex_patch_histogram/src/convex_patch_histogram.cpp
pkg/trunk/mapping/table_object_detector/src/table_object_detector.cpp
pkg/trunk/motion_planning/motion_planning_msgs/manifest.xml
pkg/trunk/motion_planning/motion_planning_msgs/msg/KinematicSpaceParameters.msg
pkg/trunk/motion_planning/motion_planning_msgs/msg/PoseConstraint.msg
pkg/trunk/motion_planning/mpglue/include/mpglue/footprint.h
pkg/trunk/motion_planning/mpglue/include/mpglue/plan.h
pkg/trunk/motion_planning/mpglue/src/costmap_planner_node.cpp
pkg/trunk/motion_planning/mpglue/src/footprint.cpp
pkg/trunk/motion_planning/mpglue/src/plan.cpp
pkg/trunk/motion_planning/mpglue/src/sbpl_environment.cpp
pkg/trunk/motion_planning/plan_ompl_path/src/plan_kinematic_path.cpp
pkg/trunk/motion_planning/planning_environment/src/monitors/collision_space_monitor.cpp
pkg/trunk/motion_planning/planning_environment/src/monitors/kinematic_model_state_monitor.cpp
pkg/trunk/motion_planning/planning_environment/src/tools/clear_known_objects.cpp
pkg/trunk/motion_planning/planning_environment/src/tools/display_planner_collision_model.cpp
pkg/trunk/motion_planning/planning_research/sbpl_arm_planner_node/include/sbpl_arm_planner_node/sbpl_arm_check_path.h
pkg/trunk/motion_planning/planning_research/sbpl_arm_planner_node/include/sbpl_arm_planner_node/sbpl_arm_planner_node.h
pkg/trunk/motion_planning/planning_research/sbpl_arm_planner_node/src/sbpl_arm_planner_node.cpp
pkg/trunk/motion_planning/planning_research/sbpl_door_planner/include/sbpl_door_planner/door_base_collision_cost.h
pkg/trunk/motion_planning/planning_research/sbpl_door_planner/include/sbpl_door_planner/environment_navxythetadoor.h
pkg/trunk/motion_planning/planning_research/sbpl_door_planner/src/discrete_space_information/navxythetadoor/door_base_collision_cost.cpp
pkg/trunk/motion_planning/planning_research/sbpl_door_planner/src/discrete_space_information/navxythetadoor/environment_navxythetadoor.cpp
pkg/trunk/motion_planning/planning_research/sbpl_door_planner/src/test/main.cpp
pkg/trunk/motion_planning/planning_research/sbpl_door_planner_action/include/sbpl_door_planner_action/sbpl_door_planner_action.h
pkg/trunk/motion_planning/planning_research/sbpl_door_planner_action/src/sbpl_door_planner_action.cpp
pkg/trunk/motion_planning/planning_research/sbpl_planner_node/include/sbpl_planner_node/sbpl_planner_node.h
pkg/trunk/motion_planning/planning_research/sbpl_planner_node/manifest.xml
pkg/trunk/motion_planning/planning_research/sbpl_planner_node/srv/PlanPathSrv.srv
pkg/trunk/nav/people_aware_nav/include/people_aware_nav/move_base_constrained.h
pkg/trunk/nav/people_aware_nav/manifest.xml
pkg/trunk/nav/people_aware_nav/msg/ConstrainedMoveBaseState.msg
pkg/trunk/nav/people_aware_nav/src/head_controller.cpp
pkg/trunk/nav/people_aware_nav/src/move_base_constrained.cpp
pkg/trunk/nav/people_aware_nav/src/navfn_constrained.cpp
pkg/trunk/nav/people_aware_nav/src/person_node.cpp
pkg/trunk/nav/people_aware_nav/srv/GlanceAt.srv
pkg/trunk/nav/teleop_base/src/ground_truth_controller.cpp
pkg/trunk/nav/visual_nav/src/ros_visual_nav.cpp
pkg/trunk/nav/wavefront/src/cli.cpp
pkg/trunk/nav/wavefront/src/wavefront_node.cpp
pkg/trunk/openrave_planning/orrosplanning/src/mocapsystem.h
pkg/trunk/openrave_planning/orrosplanning/src/objecttransformsystem.h
pkg/trunk/pr2/teleop/spacenav_node/src/spacenav_node.cpp
pkg/trunk/sandbox/actionlib/manifest.xml
pkg/trunk/sandbox/actionlib/msg/MoveBaseFeedback.msg
pkg/trunk/sandbox/actionlib/msg/MoveBaseGoal.msg
pkg/trunk/sandbox/actionlib/msg/MoveBaseResult.msg
pkg/trunk/sandbox/actionlib/src/move_base_client_old.cpp
pkg/trunk/sandbox/actionlib/src/move_base_server.cpp
pkg/trunk/sandbox/camera_calibration/manifest.xml
pkg/trunk/sandbox/camera_calibration/msg/CalibrationPattern.msg
pkg/trunk/sandbox/descriptors_3d/examples/interest_point_example.cpp
pkg/trunk/sandbox/descriptors_3d/include/descriptors_3d/bounding_box_raw.h
pkg/trunk/sandbox/descriptors_3d/include/descriptors_3d/bounding_box_spectral.h
pkg/trunk/sandbox/descriptors_3d/include/descriptors_3d/descriptor_3d.h
pkg/trunk/sandbox/descriptors_3d/include/descriptors_3d/generic/neighborhood_feature.h
pkg/trunk/sandbox/descriptors_3d/include/descriptors_3d/generic/orientation_generic.h
pkg/trunk/sandbox/descriptors_3d/include/descriptors_3d/generic/spin_image_generic.h
pkg/trunk/sandbox/descriptors_3d/include/descriptors_3d/orientation_normal.h
pkg/trunk/sandbox/descriptors_3d/include/descriptors_3d/orientation_tangent.h
pkg/trunk/sandbox/descriptors_3d/include/descriptors_3d/position.h
pkg/trunk/sandbox/descriptors_3d/include/descriptors_3d/shape_spectral.h
pkg/trunk/sandbox/descriptors_3d/include/descriptors_3d/spectral_analysis.h
pkg/trunk/sandbox/descriptors_3d/include/descriptors_3d/spin_image_custom.h
pkg/trunk/sandbox/descriptors_3d/include/descriptors_3d/spin_image_normal.h
pkg/trunk/sandbox/descriptors_3d/include/descriptors_3d/spin_image_tangent.h
pkg/trunk/sandbox/descriptors_3d/src/bounding_box_raw.cpp
pkg/trunk/sandbox/descriptors_3d/src/bounding_box_spectral.cpp
pkg/trunk/sandbox/descriptors_3d/src/descriptor_3d.cpp
pkg/trunk/sandbox/descriptors_3d/src/neighborhood_feature.cpp
pkg/trunk/sandbox/descriptors_3d/src/orientation_generic.cpp
pkg/trunk/sandbox/descriptors_3d/src/orientation_normal.cpp
pkg/trunk/sandbox/descriptors_3d/src/orientation_tangent.cpp
pkg/trunk/sandbox/descriptors_3d/src/position.cpp
pkg/trunk/sandbox/descriptors_3d/src/shape_spectral.cpp
pkg/trunk/sandbox/descriptors_3d/src/spectral_analysis.cpp
pkg/trunk/sandbox/descriptors_3d/src/spin_image_custom.cpp
pkg/trunk/sandbox/descriptors_3d/src/spin_image_generic.cpp
pkg/trunk/sandbox/descriptors_3d/src/spin_image_normal.cpp
pkg/trunk/sandbox/descriptors_3d/src/spin_image_tangent.cpp
pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/plug_controller.h
pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/trajectory_controller.h
pkg/trunk/sandbox/experimental_controllers/src/plug_controller.cpp
pkg/trunk/sandbox/fk_node/src/fk_node.cpp
pkg/trunk/sandbox/functional_m3n/examples/classify_m3n.cpp
pkg/trunk/sandbox/functional_m3n/examples/train_m3n.cpp
pkg/trunk/sandbox/functional_m3n/include/functional_m3n/example/pt_cloud_rf_creator.h
pkg/trunk/sandbox/functional_m3n/src/example/pt_cloud_rf_creator.cpp
pkg/trunk/sandbox/labeled_object_detector/include/detect_nearest_object_action.h
pkg/trunk/sandbox/labeled_object_detector/include/object_model.h
pkg/trunk/sandbox/labeled_object_detector/include/pcd_misc.h
pkg/trunk/sandbox/labeled_object_detector/include/planar_object_detector.h
pkg/trunk/sandbox/labeled_object_detector/include/planar_object_detector_node.h
pkg/trunk/sandbox/labeled_object_detector/msg/PoseStampedState.msg
pkg/trunk/sandbox/labeled_object_detector/src/detect_nearest_object_action.cpp
pkg/trunk/sandbox/labeled_object_detector/src/object_model.cpp
pkg/trunk/sandbox/labeled_object_detector/src/pcd_misc.cpp
pkg/trunk/sandbox/labeled_object_detector/src/planar_object_detector.cpp
pkg/trunk/sandbox/labeled_object_detector/src/segment_objects.cpp
pkg/trunk/sandbox/labeled_object_detector/test/test_nearest_action.cpp
pkg/trunk/sandbox/labeled_object_detector/test/test_nearest_action2.cpp
pkg/trunk/sandbox/labeled_object_detector/test/test_pcd_segmentation.cpp
pkg/trunk/sandbox/lifelong_mapping/msg/TransformWithCovariance.msg
pkg/trunk/sandbox/message_filters/src/filter_example.cpp
pkg/trunk/sandbox/move_base_client/src/move_base_client.cpp
pkg/trunk/sandbox/move_base_client/src/move_base_translator.cpp
pkg/trunk/sandbox/move_base_client/src/simple_move_base_client.cpp
pkg/trunk/sandbox/mturk_grab_object/include/mturk_grab_object/grasp_table_object_node.h
pkg/trunk/sandbox/mturk_grab_object/src/grasp_table_object.cpp
pkg/trunk/sandbox/object_modeler/src/collect_data.cpp
pkg/trunk/sandbox/pcd_novelty/include/novelty_estimator.h
pkg/trunk/sandbox/pcd_novelty/include/novelty_estimator_node.h
pkg/trunk/sandbox/pcd_novelty/src/novelty_estimator.cpp
pkg/trunk/sandbox/pcd_novelty/src/novelty_estimator_node.cpp
pkg/trunk/sandbox/pcd_novelty/test/test_novelty_estimator.cpp
pkg/trunk/sandbox/person_follower/src/follower.cpp
pkg/trunk/sandbox/planar_objects/manifest.xml
pkg/trunk/sandbox/planar_objects/msg/BoxObservation.msg
pkg/trunk/sandbox/planar_objects/src/box_detector.cpp
pkg/trunk/sandbox/planar_objects/src/box_detector.h
pkg/trunk/sandbox/planar_objects/src/cornercandidate.cpp
pkg/trunk/sandbox/planar_objects/src/cornercandidate.h
pkg/trunk/sandbox/planar_objects/src/find_planes.cpp
pkg/trunk/sandbox/planar_objects/src/find_planes.h
pkg/trunk/sandbox/planar_objects/src/rectangular_fit.cpp
pkg/trunk/sandbox/planar_objects/src/rectangular_fit.h
pkg/trunk/sandbox/planar_objects/src/vis_utils.cpp
pkg/trunk/sandbox/planar_objects/src/vis_utils.h
pkg/trunk/sandbox/point_cloud_clustering/include/point_cloud_clustering/kmeans.h
pkg/trunk/sandbox/point_cloud_clustering/include/point_cloud_clustering/pairwise_neighbors.h
pkg/trunk/sandbox/point_cloud_clustering/include/point_cloud_clustering/point_cloud_clustering.h
pkg/trunk/sandbox/point_cloud_clustering/src/kmeans.cpp
pkg/trunk/sandbox/point_cloud_clustering/src/pairwise_neighbors.cpp
pkg/trunk/sandbox/point_cloud_clustering/src/point_cloud_clustering.cpp
pkg/trunk/sandbox/point_cloud_tutorials/src/planar_fit.cpp
pkg/trunk/sandbox/point_cloud_tutorials/src/plane_fit.cpp
pkg/trunk/sandbox/poseviz/src/poseviz.cpp
pkg/trunk/sandbox/rf_detector/include/ObjectInPerspective.h
pkg/trunk/sandbox/rf_detector/src/ObjectInPerspective.cpp
pkg/trunk/sandbox/rf_detector/src/RFDetector.cpp
pkg/trunk/sandbox/teleop_anti_collision/include/teleop_anti_collision/anti_collision_base_controller.h
pkg/trunk/sandbox/teleop_anti_collision/src/anti_collision_base_controller.cpp
pkg/trunk/sandbox/teleop_anti_collision/src/teleop_goal_projection.cpp
pkg/trunk/sandbox/test_arm_vision_calibration/src/test_arm_vision_calibration.cpp
pkg/trunk/sandbox/tf_conversions/include/tf_conversions/tf_kdl.h
pkg/trunk/sandbox/tf_conversions/src/tf_kdl.cpp
pkg/trunk/sandbox/tf_node/manifest.xml
pkg/trunk/sandbox/tf_node/msg/StartStaticTransform.msg
pkg/trunk/sandbox/tf_node/src/tf_node.cpp
pkg/trunk/sandbox/tf_node/srv/TransformPoint.srv
pkg/trunk/sandbox/tf_node/srv/TransformPointCloud.srv
pkg/trunk/sandbox/tf_node/srv/TransformPose.srv
pkg/trunk/sandbox/tf_node/srv/TransformQuaternion.srv
pkg/trunk/sandbox/tf_node/srv/TransformVector.srv
pkg/trunk/sandbox/voxel3d/include/voxel3d/voxel3d.h
pkg/trunk/sandbox/voxel3d/manifest.xml
pkg/trunk/sandbox/voxel3d/msg/DistanceField.msg
pkg/trunk/sandbox/voxel3d/src/voxel3d.cpp
pkg/trunk/sandbox/voxel3d/src/voxel_node.cpp
pkg/trunk/stacks/camera_drivers/dcam/src/nodes/dcam.cpp
pkg/trunk/stacks/common/laser_scan/include/laser_scan/footprint_filter.h
pkg/trunk/stacks/common/laser_scan/include/laser_scan/laser_processor.h
pkg/trunk/stacks/common/laser_scan/include/laser_scan/laser_scan.h
pkg/trunk/stacks/common/laser_scan/include/laser_scan/point_cloud_footprint_filter.h
pkg/trunk/stacks/common/laser_scan/src/laser_processor.cpp
pkg/trunk/stacks/common/laser_scan/src/laser_scan.cc
pkg/trunk/stacks/common/laser_scan/src/scan_shadows_filter.cpp
pkg/trunk/stacks/common/laser_scan/src/scan_to_cloud.cpp
pkg/trunk/stacks/common/laser_scan/test/projection_test.cpp
pkg/trunk/stacks/common/manipulation_msgs/manifest.xml
pkg/trunk/stacks/common/manipulation_msgs/msg/IKRequest.msg
pkg/trunk/stacks/common/manipulation_msgs/msg/TaskFrameFormalism.msg
pkg/trunk/stacks/common/manipulation_srvs/manifest.xml
pkg/trunk/stacks/common/python_actions/src/exec.cpp
pkg/trunk/stacks/common/python_actions/src/tiny_action.cpp
pkg/trunk/stacks/common_msgs/diagnostic_msgs/manifest.xml
pkg/trunk/stacks/common_msgs/diagnostic_msgs/msg/DiagnosticStatus.msg
pkg/trunk/stacks/common_msgs/mapping_msgs/manifest.xml
pkg/trunk/stacks/common_msgs/mapping_msgs/msg/AttachedObject.msg
pkg/trunk/stacks/common_msgs/mapping_msgs/msg/Object.msg
pkg/trunk/stacks/common_msgs/mapping_msgs/msg/ObjectInMap.msg
pkg/trunk/stacks/common_msgs/mapping_msgs/msg/OrientedBoundingBox.msg
pkg/trunk/stacks/common_msgs/mapping_msgs/msg/PolygonalMap.msg
pkg/trunk/stacks/common_msgs/nav_msgs/manifest.xml
pkg/trunk/stacks/common_msgs/nav_msgs/msg/MapMetaData.msg
pkg/trunk/stacks/common_msgs/nav_msgs/msg/PoseArray.msg
pkg/trunk/stacks/common_msgs/nav_msgs/srv/GetMap.srv
pkg/trunk/stacks/common_msgs/nav_msgs/srv/GetPlan.srv
pkg/trunk/stacks/common_msgs/robot_msgs/manifest.xml
pkg/trunk/stacks/common_msgs/robot_msgs/msg/Path.msg
pkg/trunk/stacks/common_msgs/robot_msgs/msg/Polygon3D.msg
pkg/trunk/stacks/common_msgs/robot_msgs/msg/VOPose.msg
pkg/trunk/stacks/common_msgs/robot_msgs/msg/Wrench.msg
pkg/trunk/stacks/common_msgs/robot_srvs/manifest.xml
pkg/trunk/stacks/common_msgs/robot_srvs/srv/SetPoseStamped.srv
pkg/trunk/stacks/common_msgs/sensor_msgs/manifest.xml
pkg/trunk/stacks/common_msgs/test_common_msgs/manifest.xml
pkg/trunk/stacks/common_msgs/test_common_msgs/test/test_common_msgs_migration.py
pkg/trunk/stacks/estimation/robot_pose_ekf/include/robot_pose_ekf/odom_estimation.h
pkg/trunk/stacks/estimation/robot_pose_ekf/include/robot_pose_ekf/odom_estimation_node.h
pkg/trunk/stacks/estimation/robot_pose_ekf/manifest.xml
pkg/trunk/stacks/estimation/robot_pose_ekf/src/odom_estimation.cpp
pkg/trunk/stacks/estimation/robot_pose_ekf/src/odom_estimation_node.cpp
pkg/trunk/stacks/estimation/robot_pose_ekf/test/test_robot_pose_ekf.cpp
pkg/trunk/stacks/ethercat_drivers/fingertip_pressure/manifest.xml
pkg/trunk/stacks/ethercat_drivers/fingertip_pressure/msg/PressureInfoElement.msg
pkg/trunk/stacks/geometry/tf/include/tf/transform_datatypes.h
pkg/trunk/stacks/geometry/tf/include/tf/transform_listener.h
pkg/trunk/stacks/geometry/tf/manifest.xml
pkg/trunk/stacks/geometry/tf/msg/tfMessage.msg
pkg/trunk/stacks/geometry/tf/src/change_notifier.cpp
pkg/trunk/stacks/geometry/tf/src/tf.cpp
pkg/trunk/stacks/geometry/tf/src/transform_broadcaster.cpp
pkg/trunk/stacks/geometry/tf/src/transform_listener.cpp
pkg/trunk/stacks/geometry/tf/test/testPython.py
pkg/trunk/stacks/geometry/tf/test/test_message_filter.cpp
pkg/trunk/stacks/geometry/tf/test/test_notifier.cpp
pkg/trunk/stacks/geometry/tf/test/tf_unittest.cpp
pkg/trunk/stacks/geometry/tf_tutorials/src/shuttle_observer.cpp
pkg/trunk/stacks/geometry/tf_tutorials/src/tutorial_listener.cpp
pkg/trunk/stacks/imu_drivers/imu_node/imu_node.cc
pkg/trunk/stacks/mechanism/robot_state_publisher/src/robot_state_publisher.cpp
pkg/trunk/stacks/navigation/amcl/src/amcl_node.cpp
pkg/trunk/stacks/navigation/base_local_planner/include/base_local_planner/costmap_model.h
pkg/trunk/stacks/navigation/base_local_planner/include/base_local_planner/planar_laser_scan.h
pkg/trunk/stacks/navigation/base_local_planner/include/base_local_planner/point_grid.h
pkg/trunk/stacks/navigation/base_local_planner/include/base_local_planner/trajectory_planner.h
pkg/trunk/stacks/navigation/base_local_planner/include/base_local_planner/trajectory_planner_ros.h
pkg/trunk/stacks/navigation/base_local_planner/include/base_local_planner/voxel_grid_model.h
pkg/trunk/stacks/navigation/base_local_planner/include/base_local_planner/world_model.h
pkg/trunk/stacks/navigation/base_local_planner/src/costmap_model.cpp
pkg/trunk/stacks/navigation/base_local_planner/src/point_grid.cpp
pkg/trunk/stacks/navigation/base_local_planner/src/trajectory_planner.cpp
pkg/trunk/stacks/navigation/base_local_planner/src/trajectory_planner_ros.cpp
pkg/trunk/stacks/navigation/base_local_planner/src/voxel_grid_model.cpp
pkg/trunk/stacks/navigation/carrot_planner/include/carrot_planner/carrot_planner.h
pkg/trunk/stacks/navigation/carrot_planner/src/carrot_planner.cpp
pkg/trunk/stacks/navigation/costmap_2d/include/costmap_2d/costmap_2d.h
pkg/trunk/stacks/navigation/costmap_2d/include/costmap_2d/costmap_2d_ros.h
pkg/trunk/stacks/navigation/costmap_2d/include/costmap_2d/observation.h
pkg/trunk/stacks/navigation/costmap_2d/include/costmap_2d/observation_buffer.h
pkg/trunk/stacks/navigation/costmap_2d/include/costmap_2d/voxel_costmap_2d.h
pkg/trunk/stacks/navigation/costmap_2d/manifest.xml
pkg/trunk/stacks/navigation/costmap_2d/msg/VoxelGrid.msg
pkg/trunk/stacks/navigation/costmap_2d/src/costmap_2d.cpp
pkg/trunk/stacks/navigation/costmap_2d/src/costmap_2d_cloud.cpp
pkg/trunk/stacks/navigation/costmap_2d/src/costmap_2d_markers.cpp
pkg/trunk/stacks/navigation/costmap_2d/src/costmap_2d_ros.cpp
pkg/trunk/stacks/navigation/costmap_2d/src/observation_buffer.cpp
pkg/trunk/stacks/navigation/costmap_2d/src/voxel_costmap_2d.cpp
pkg/trunk/stacks/navigation/fake_localization/fake_localization.cpp
pkg/trunk/stacks/navigation/map_server/src/map_saver.cpp
pkg/trunk/stacks/navigation/move_base/include/move_base/move_base.h
pkg/trunk/stacks/navigation/move_base/include/move_base/move_base_old.h
pkg/trunk/stacks/navigation/move_base/manifest.xml
pkg/trunk/stacks/navigation/move_base/msg/MoveBaseFeedback.msg
pkg/trunk/stacks/navigation/move_base/msg/MoveBaseGoal.msg
pkg/trunk/stack...
[truncated message content] |
|
From: <bla...@us...> - 2009-08-04 08:13:54
|
Revision: 20636
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=20636&view=rev
Author: blaisegassend
Date: 2009-08-04 08:13:45 +0000 (Tue, 04 Aug 2009)
Log Message:
-----------
Improved automatic documentation generation in dynamic_reconfigure.
Converted the forearm_cam node to URL-based camera naming.
Modified Paths:
--------------
pkg/trunk/drivers/cam/forearm_cam/cfg/ForearmCam.cfg
pkg/trunk/drivers/cam/forearm_cam/forearm_cam.launch
pkg/trunk/drivers/cam/forearm_cam/include/fcamlib.h
pkg/trunk/drivers/cam/forearm_cam/manifest.xml
pkg/trunk/drivers/cam/forearm_cam/prf_forearm_cam_config.launch
pkg/trunk/drivers/cam/forearm_cam/src/fcamlib/fcamlib.c
pkg/trunk/drivers/cam/forearm_cam/src/fcamlib/host_netutil.c
pkg/trunk/drivers/cam/forearm_cam/src/fcamlib/list.c
pkg/trunk/drivers/cam/forearm_cam/src/nodes/forearm_node.cpp
pkg/trunk/stacks/driver_common/dynamic_reconfigure/src/dynamic_reconfigure/parameter_generator.py
Added Paths:
-----------
pkg/trunk/drivers/cam/forearm_cam/dox/
pkg/trunk/drivers/cam/forearm_cam/dox/doc.dox
Modified: pkg/trunk/drivers/cam/forearm_cam/cfg/ForearmCam.cfg
===================================================================
--- pkg/trunk/drivers/cam/forearm_cam/cfg/ForearmCam.cfg 2009-08-04 07:23:59 UTC (rev 20635)
+++ pkg/trunk/drivers/cam/forearm_cam/cfg/ForearmCam.cfg 2009-08-04 08:13:45 UTC (rev 20636)
@@ -39,23 +39,20 @@
from dynamic_reconfigure.msg import SensorLevels
from dynamic_reconfigure.parameter_generator import *
-gen = ParameterGenerator(PACKAGE, "ForearmCam")
+gen = ParameterGenerator(PACKAGE, "forearm_node", "ForearmCam")
# Name Type Reconfiguration level Description Default Min Max
-gen.add("exit_on_fault", bool_t, SensorLevels.RECONFIGURE_RUNNING, "Indicates if the driver should exit when an error occurs.", False)
-gen.add("if_name", str_t, SensorLevels.RECONFIGURE_CLOSE, "The network interface to which the camera is connected. Use \"auto\" for automatic.", "auto")
-gen.add("ip_address", str_t, SensorLevels.RECONFIGURE_CLOSE, "The ip address to which the camera will be configured. Use \"auto\" for automatic.", "auto")
+#gen.add("exit_on_fault", bool_t, SensorLevels.RECONFIGURE_RUNNING, "Indicates if the driver should exit when an error occurs.", False)
gen.add("auto_gain", bool_t, SensorLevels.RECONFIGURE_CLOSE, "Sets the analog gain to automatic. Causes the @b gain setting to be ignored.", True)
gen.add("gain", int_t, SensorLevels.RECONFIGURE_CLOSE, "The camera analog gain.", 32, 16, 64)
-gen.add("port", int_t, SensorLevels.RECONFIGURE_CLOSE, "The local port the camera should connect to. Defaults to 9090.", 9090)
gen.add("ext_trig", bool_t, SensorLevels.RECONFIGURE_CLOSE, "Set camera to trigger from the external trigger input.", False)
gen.add("video_mode", str_t, SensorLevels.RECONFIGURE_CLOSE, "Sets the camera video mode.", "640x480x30")
gen.add("frame_id", str_t, SensorLevels.RECONFIGURE_RUNNING, "Sets the TF frame from which the camera is publishing.", "")
gen.add("trig_controller", str_t, SensorLevels.RECONFIGURE_CLOSE, "Sets the trigger controller from which an externally trigged camera operates.", "")
gen.add("trig_rate", double_t, SensorLevels.RECONFIGURE_CLOSE, "Sets the triggering rate in externally triggered mode.", 30)
gen.add("trig_phase", int_t, SensorLevels.RECONFIGURE_CLOSE, "Sets the phase of the external trigger signal.", 0, 0, 1)
-gen.add("serial_number", int_t, SensorLevels.RECONFIGURE_CLOSE, "Sets the serial number to use. Use -1 for automatic. The default of -2 will always fail.", -2)
gen.add("first_packet_offset", double_t, SensorLevels.RECONFIGURE_RUNNING, "Offset between the end of exposure and the minimal arrival time for the first frame packet.", 0.0025, 0)
gen.add("auto_exposure", bool_t, SensorLevels.RECONFIGURE_CLOSE, "Sets the camera exposure duration to automatic. Causes the @b exposure setting to be ignored.", True)
gen.add("exposure", double_t, SensorLevels.RECONFIGURE_CLOSE, "Sets the camera exposure The valid range depends on the video mode.", 0, 0.01)
+gen.add("camera_url", str_t, SensorLevels.RECONFIGURE_CLOSE, "URL defining which camera to connect to, on what interface and with which IP address.", 'any://')
exit(gen.generate())
Added: pkg/trunk/drivers/cam/forearm_cam/dox/doc.dox
===================================================================
--- pkg/trunk/drivers/cam/forearm_cam/dox/doc.dox (rev 0)
+++ pkg/trunk/drivers/cam/forearm_cam/dox/doc.dox 2009-08-04 08:13:45 UTC (rev 20636)
@@ -0,0 +1,118 @@
+/**
+
+
+@section topic Forearm Camera ROS Topics
+ - forearm_node
+
+ */
+
+
+/**
+\mainpage
+\htmlinclude manifest.html
+
+The forearm camera is an ethernet camera located in the forearm of the PR2.
+This page describes the API of the camera.
+
+A detailed specification of the camera can be found here:
+http://pr.willowgarage.com/wiki/Ethernet_Forearm_Camera
+
+The forearm_cam package documentation with tutorials and background
+information can be found here:
+http://pr.willowgarage.com/wiki/forearm_cam
+
+The foream camera can be used in triggered mode. Detailed documentation of
+the triggered mode can be found here:
+http://pr.willowgarage.com/wiki/camera_synchronization
+
+\subsection urls Forearm Camera URLs
+
+Forearm cameras and their network configuration are described by URLs in
+the following form:
+
+\verbatim
+name://camera_name[@camera_ip][#local_interface]
+serial://serial_number[@camera_ip][#local_interface]
+any://[@camera_ip][#local_interface]
+\endverbatim
+
+A \b name URL indicates the name of the camera to be found. A \b serial URL
+indicates the serial number of the camera to be found. An \b any URL will
+match any camera. If a URL matches more than one camera, the node will
+refuse to start the camera.
+
+Optionally, the ip address that the camera should be set to can be
+specified by prefixing it with a @ sign, and the interface through
+which to access the camera can be specified by prefixing it with a #
+sign.
+
+Some example URLs:
+
+\verbatim
+any:// // Matches any camera
+serial://15#eth2 // Matches the camera with s/n 15, and only looks for it on interface eth2
+name://left_forearm@10.68.0.210 // Matches the \b left_forearm camera, and sets its IP to 10.68.0.210
+\endverbatim
+
+\subsection forearm_node forearm_node
+
+The forearm camera driver. Connects to the camera and streams out images.
+
+\include ForearmCamConfig-usage.dox
+
+\subsubsection topics ROS topics
+
+Subscribes to:
+- none
+
+Publishes to:
+- \b "~image_raw": [sensor_msgs/Image] The raw image stream from the
+ camera.
+
+- \b "~cam_info": [sensor_msgs/CameraInfo] The camera's calibration
+ information.
+
+\include ForearmCamConfig.dox
+
+\section commandline Command-line tools
+
+- discover Finding cameras on the network
+
+\subsection discover discover
+
+A tool to \b discover cameras that are visible from the computer on which it
+is run.
+
+The \b discover tool broadcasts on one or all interfaces to discover
+cameras that are reachable. Its output indicates the camera's serial
+number, name (if it has one), MAC address, interface, currently configured
+IP address, and revision information.
+
+Notes:
+
+* If a camera is visible from multiple network interfaces, only the first one
+that responds will be shown.
+
+* The IP address returned by the discover tool is the camera's currently
+configured address. This may not be a legal address on the current
+network. This may not be the address stored in the camera's flash. This
+is in order of precedence: the last address that the camera was
+configured with, the address stored in the camera's flash memory the last
+time the camera was rebooted, the default address 167.254.0.1.
+
+* Cameras on a network interface that is not properly configured for
+IPv4 traffic will not be seen by the discover tool.
+
+\subsubsection Usage
+
+To discover all cameras that are visible:
+\verbatim
+rosrun forearm_cam discover
+\endverbatim
+
+To discover all cameras that are visible on interface \b eth1:
+\verbatim
+rosrun forearm_cam discover eth1
+\endverbatim
+
+*/
Modified: pkg/trunk/drivers/cam/forearm_cam/forearm_cam.launch
===================================================================
--- pkg/trunk/drivers/cam/forearm_cam/forearm_cam.launch 2009-08-04 07:23:59 UTC (rev 20635)
+++ pkg/trunk/drivers/cam/forearm_cam/forearm_cam.launch 2009-08-04 08:13:45 UTC (rev 20636)
@@ -1,10 +1,11 @@
<launch>
<group ns="forearm">
- <param name="if_name" type="str" value=""/>
- <param name="ip_address" type="str" value="10.0.1.123"/>
- <param name="serial_number" type="int" value="-1"/>
+ <!--param name="if_name" type="str" value=""/-->
+ <!--param name="ip_address" type="str" value="10.0.1.123"/-->
+ <!--param name="serial_number" type="int" value="-1"/-->
+ <!--param name="frame_id" type="str" value="???"/-->
+ <param name="camera_url" type="str" value="name://Camera13@10.0.1.123"/>
<param name="video_mode" type="str" value="640x480x30"/>
- <!--param name="frame_id" type="str" value="???"/-->
<param name="do_colorize" type="bool" value="True"/>
<param name="do_rectify" type="bool" value="True"/>
<param name="ext_trigger" type="bool" value="False"/>
Modified: pkg/trunk/drivers/cam/forearm_cam/include/fcamlib.h
===================================================================
--- pkg/trunk/drivers/cam/forearm_cam/include/fcamlib.h 2009-08-04 07:23:59 UTC (rev 20635)
+++ pkg/trunk/drivers/cam/forearm_cam/include/fcamlib.h 2009-08-04 08:13:45 UTC (rev 20636)
@@ -74,6 +74,7 @@
int fcamLibVersion( void );
+int fcamFindByUrl(const char *url, IpCamList *camera, unsigned wait_us, const char **errmsg);
int fcamDiscover(const char *ifName, IpCamList *ipCamList, const char *ipAddress, unsigned wait_us);
int fcamConfigure( IpCamList *camInfo, const char *ipAddress, unsigned wait_us);
int fcamStartVid( const IpCamList *camInfo, const uint8_t mac[6], const char *ipAddress, uint16_t port );
Modified: pkg/trunk/drivers/cam/forearm_cam/manifest.xml
===================================================================
--- pkg/trunk/drivers/cam/forearm_cam/manifest.xml 2009-08-04 07:23:59 UTC (rev 20635)
+++ pkg/trunk/drivers/cam/forearm_cam/manifest.xml 2009-08-04 08:13:45 UTC (rev 20636)
@@ -1,6 +1,7 @@
<package>
<description>A library and ROS node to provide access to the forearm camera from MindTribe.</description>
- <author>Patrick Mihelich, Eric MacIntosh, David Palchak, Blaise Gassend</author>
+ <author>Blaise Gassend, Patrick Mihelich, Eric MacIntosh, David Palchak</author>
+ <url>http://pr.willowgarage.com/wiki/forearm_cam</url>
<license>BSD</license>
<review status="unreviewed" notes=""/>
<depend package="roscpp" />
Modified: pkg/trunk/drivers/cam/forearm_cam/prf_forearm_cam_config.launch
===================================================================
--- pkg/trunk/drivers/cam/forearm_cam/prf_forearm_cam_config.launch 2009-08-04 07:23:59 UTC (rev 20635)
+++ pkg/trunk/drivers/cam/forearm_cam/prf_forearm_cam_config.launch 2009-08-04 08:13:45 UTC (rev 20636)
@@ -1,8 +1,6 @@
<launch>
<group ns="forearm_cam_r">
- <param name="if_name" type="str" value="eth0"/>
- <param name="ip_address" type="str" value="10.68.0.68"/>
- <param name="serial_number" type="int" value="3"/>
+ <param name="camera_url" type="str" value="serial://3@10.68.0.68"/>
<param name="video_mode" type="str" value="640x480x30"/>
<param name="frame_id" type="str" value="r_forearm_cam_optical_frame"/>
<param name="do_colorize" type="bool" value="True"/>
Modified: pkg/trunk/drivers/cam/forearm_cam/src/fcamlib/fcamlib.c
===================================================================
--- pkg/trunk/drivers/cam/forearm_cam/src/fcamlib/fcamlib.c 2009-08-04 07:23:59 UTC (rev 20635)
+++ pkg/trunk/drivers/cam/forearm_cam/src/fcamlib/fcamlib.c 2009-08-04 08:13:45 UTC (rev 20636)
@@ -42,6 +42,170 @@
/**
+ * Finds a camera matching the given name
+ *
+ * Names are of the form:
+\verbatim
+name://camera_name[@camera_ip][#local_interface]
+serial://serial_number[@camera_ip][#local_interface]
+any://[@camera_ip][#local_interface]
+\endverbatim
+ *
+ * A name URL indicates the name of the camera to be found. A serial URL
+ * indicates the serial number of the camera to be found. An any URL will
+ * match any camera, but if more than one camera is found, it will fail.
+ *
+ * Optionally, the ip address that the camera should be set to can be
+ * specified by prefixing it with a @ sign, and the interface through
+ * which to access the camera can be specified by prefixing it with a #
+ * sign.
+ *
+ * @param url The url of the camera to find
+ * @param camera The structure to fill the camera information into
+ * @param wait_us The number of microseconds to wait before timing out
+ * @param errmsg String containing a descriptive parse error message
+ *
+ * @return Returns 1 if a camera was found, 2 if multiple cameras were found,
+ * 0 if no cameras were found
+ * and -1 with errno set if a system errors occured.
+ */
+int fcamFindByUrl(const char *url, IpCamList *camera, unsigned wait_us, const char **errmsg)
+{
+ static const char *badprefix = "Bad URL prefix, expected name://, serial:// or any://.";
+ static const char *multiaddress = "Multiple @-prefixed camera addresses found.";
+ static const char *multiinterface = "Multiple #-prefixed host interfaces found.";
+ static const char *discoverfailed = "Discover failed. The specified address or interface may be bad.";
+ static const char *badserial = "serial:// may only be followed by an integer.";
+ static const char *badip = "@ must be followed by a valid IP address.";
+ const char *name = "name://"; int namelen = strlen(name);
+ const char *serial = "serial://"; int seriallen = strlen(serial);
+ const char *any = "any://"; int anylen = strlen(any);
+ char *idstart;
+ char *curpos;
+ char *address = NULL;
+ char *interface = NULL;
+ char *copy = malloc(strlen(url));
+ int retval = -1;
+ int mode = 0;
+ unsigned int serialnum = -1;
+ IpCamList camList;
+ IpCamList *curEntry;
+
+ fcamCamListInit(&camList); // Must happen before any potential failures.
+
+ errmsg = NULL;
+
+ if (!copy)
+ return -1;
+
+ strcpy(copy, url);
+
+ // Decypher the prefix.
+ if (!strncmp(copy, name, namelen))
+ {
+ idstart = copy + namelen;
+ mode = 1;
+ }
+ else if (!strncmp(copy, serial, seriallen))
+ {
+ idstart = copy + seriallen;
+ mode = 2;
+ }
+ else if (!strncmp(copy, any, anylen))
+ {
+ idstart = copy + anylen;
+ mode = 3;
+ }
+ else
+ {
+ *errmsg = badprefix;
+ goto bailout;
+ }
+
+ // Find any # or @ words.
+ for (curpos = idstart; *curpos; curpos++)
+ {
+ if (*curpos == '@')
+ {
+ if (address)
+ {
+ *errmsg = multiaddress;
+ goto bailout;
+ }
+ address = curpos + 1;
+ }
+ else if (*curpos == '#')
+ {
+ if (interface)
+ {
+ *errmsg = multiinterface;
+ goto bailout;
+ }
+ interface = curpos + 1;
+ }
+ else
+ continue; // Didn't find anything, don't terminate the string.
+ *curpos = 0;
+ }
+ // Now idstart, address and interface are set.
+
+ if (mode == 2) // serial:// convert the number to integer.
+ {
+ char *endpos;
+ serialnum = strtol(idstart, &endpos, 10);
+ if (*idstart == 0 || *endpos != 0)
+ {
+ *errmsg = badserial;
+ goto bailout;
+ }
+ }
+
+ // Build up a list of cameras. Only ones that are on the specified
+ // interface (if specified), and that can reply from the specified
+ // address (if specified) will end up in the list.
+ if (fcamDiscover(interface, &camList, address, wait_us) == -1)
+ {
+ *errmsg = discoverfailed;
+ goto bailout;
+ }
+
+ // Now search through the list for a match.
+ retval = 0; // From now on retval is the number of matches.
+ list_for_each_entry(curEntry, &(camList.list), list)
+ {
+ if ((mode == 1 && strcmp(idstart, curEntry->cam_name) == 0) ||
+ (mode == 2 && serialnum == curEntry->serial) ||
+ mode == 3)
+ {
+ retval++;
+ if (retval == 2)
+ {
+ goto bailout;
+ }
+ memcpy(camera, curEntry, sizeof(IpCamList));
+ if (address) // If an address has been specified, we fill it into the camera info.
+ {
+ struct in_addr ip;
+ if (inet_aton(address, &ip))
+ camera->ip = ip.s_addr;
+ else
+ {
+ fprintf(stderr, "Camera IP address is %s.", address);
+ *errmsg = badip;
+ retval = -1;
+ goto bailout;
+ }
+ }
+ }
+ }
+
+bailout:
+ fcamCamListDelAll(&camList);
+ free(copy);
+ return retval;
+}
+
+/**
* Waits for a FCAM StatusPacket on the specified socket for a specified duration.
*
* The Status type and code will be reported back to the called via the 'type' & 'code'
@@ -1583,7 +1747,7 @@
return -1;
}
- fcamVidReceiveSocket( s, height, width, frameHandler, userData);
+ return fcamVidReceiveSocket( s, height, width, frameHandler, userData);
}
int fcamVidReceiveAuto( IpCamList *camera, size_t height, size_t width, FrameHandler frameHandler, void *userData ) {
@@ -1626,7 +1790,7 @@
}
port = ntohs(((struct sockaddr_in *)&localPort)->sin_port);
- fprintf(stderr, "Streaming to port %i.\n", port);
+// fprintf(stderr, "Streaming to port %i.\n", port);
if ( fcamStartVid( camera, (uint8_t *)&(localMac.sa_data[0]), inet_ntoa(localIp), port) != 0 )
{
Modified: pkg/trunk/drivers/cam/forearm_cam/src/fcamlib/host_netutil.c
===================================================================
--- pkg/trunk/drivers/cam/forearm_cam/src/fcamlib/host_netutil.c 2009-08-04 07:23:59 UTC (rev 20635)
+++ pkg/trunk/drivers/cam/forearm_cam/src/fcamlib/host_netutil.c 2009-08-04 08:13:45 UTC (rev 20636)
@@ -50,7 +50,7 @@
strncpy(arp.arp_dev, camInfo->ifName, sizeof(arp.arp_dev));
if( ioctl(s, SIOCSARP, &arp) == -1 ) {
- perror("Warning, was unable to create ARP entry (are you root?)");
+ //perror("Warning, was unable to create ARP entry (are you root?)");
return -1;
} else {
debug("Camera %u successfully configured\n", camInfo->serial);
Modified: pkg/trunk/drivers/cam/forearm_cam/src/fcamlib/list.c
===================================================================
--- pkg/trunk/drivers/cam/forearm_cam/src/fcamlib/list.c 2009-08-04 07:23:59 UTC (rev 20635)
+++ pkg/trunk/drivers/cam/forearm_cam/src/fcamlib/list.c 2009-08-04 08:13:45 UTC (rev 20636)
@@ -166,5 +166,4 @@
list_del(pos);
free(tmpListItem);
}
- return 0;
}
Modified: pkg/trunk/drivers/cam/forearm_cam/src/nodes/forearm_node.cpp
===================================================================
--- pkg/trunk/drivers/cam/forearm_cam/src/nodes/forearm_node.cpp 2009-08-04 07:23:59 UTC (rev 20635)
+++ pkg/trunk/drivers/cam/forearm_cam/src/nodes/forearm_node.cpp 2009-08-04 08:13:45 UTC (rev 20636)
@@ -221,9 +221,10 @@
// Camera information
std::string hwinfo_;
- int serial_number_;
- IpCamList* camera_;
- IpCamList camList;
+ IpCamList camera_;
+ std::string interface_;
+ std::string address_;
+// IpCamList camList;
// Trigger information
std::string trig_controller_cmd_;
@@ -240,13 +241,12 @@
public:
typedef forearm_cam::ForearmCamReconfigurator Reconfigurator;
- ForearmCamDriver() :
- camera_(NULL)
+ ForearmCamDriver()
{
misfire_blank_ = 0;
// Create a new IpCamList to hold the camera list
- fcamCamListInit(&camList);
+// fcamCamListInit(&camList);
// Clear statistics
last_image_time_ = 0;
@@ -272,12 +272,10 @@
height_ = MT9VModes[video_mode_].height;
imager_freq_ = MT9VModes[video_mode_].fps;
- // Distinguish between serial_number_ and config_.serial_number_ so
+ // Distinguish between camera_.serial and config_.camera_.serial so
// that when auto is selected, we will try to restart the previous
// camera before probing again.
- serial_number_ = config_.serial_number;
-
desired_freq_ = config_.ext_trig ? config_.trig_rate : imager_freq_;
if (!config_.auto_exposure && config_.exposure > 1 / desired_freq_)
@@ -295,7 +293,7 @@
~ForearmCamDriver()
{
- fcamCamListDelAll(&camList);
+// fcamCamListDelAll(&camList);
close();
}
@@ -318,10 +316,10 @@
}
// Forget results of previous discovery.
- camera_ = NULL;
- fcamCamListDelAll(&camList);
+// camera_ = NULL;
+// fcamCamListDelAll(&camList);
- // Discover any connected cameras, wait for 0.5 second for replies
+/* // Discover any connected cameras, wait for 0.5 second for replies
if( fcamDiscover(config_.if_name.c_str(), &camList, config_.ip_address.c_str(), SEC_TO_USEC(0.5)) == -1) {
ROS_FATAL("Discover error");
//node_handle_.shutdown();
@@ -335,7 +333,7 @@
// Open camera with requested serial number.
int index = -1;
- if (serial_number_ == -1) // Auto
+ if (camera_.serial == -1) // Auto
{
if (fcamCamListNumEntries(&camList) == 1)
{
@@ -346,15 +344,15 @@
ROS_FATAL("Camera autodetection only works when exactly one camera is discoverable. Unfortunately, we found %i cameras.", fcamCamListNumEntries(&camList));
}
}
- else if (serial_number_ == -2) // Nothing specified
+ else if (camera_.serial == -2) // Nothing specified
{
ROS_FATAL("No camera serial_number was specified. Specifying a serial number is now mandatory to avoid accidentally configuring a random camera elsewhere in the building. You can specify -1 for autodetection.");
}
else
{
- index = fcamCamListFind(&camList, serial_number_);
+ index = fcamCamListFind(&camList, camera_.serial);
if (index == -1) {
- ROS_FATAL("Couldn't find camera with S/N %i", serial_number_);
+ ROS_FATAL("Couldn't find camera with S/N %i", camera_.serial);
}
}
@@ -365,14 +363,40 @@
for (int i = 0; i < fcamCamListNumEntries(&camList); i++)
{
camera_ = fcamCamListGetEntry(&camList, i);
- ROS_FATAL("Found camera with S/N #%u", camera_->serial);
+ ROS_FATAL("Found camera with S/N #%u", camera_.serial);
}
return;
+ }*/
+
+ const char *errmsg;
+ int findResult = fcamFindByUrl(config_.camera_url.c_str(), &camera_, SEC_TO_USEC(0.1), &errmsg);
+ uint8_t *ip = (uint8_t *) &camera_.ip;
+ switch (findResult)
+ {
+ case 0:
+ ROS_ERROR("No camera matching %s was found.", config_.camera_url.c_str());
+ return;
+
+ case 1:
+ interface_ = camera_.ifName;
+ address_ = (boost::format("%i.%i.%i.%i")%(int)ip[0]%(int)ip[1]%(int)ip[2]%(int)ip[3]).str();
+ break;
+
+ case 2:
+ ROS_ERROR("More than one camera matched %s. Use the discover tool to determine a non-unique url for the camera.", config_.camera_url.c_str());
+ return;
+
+ case -1:
+ ROS_ERROR("Camera search failed: %s", errmsg);
+ return;
+
+ default:
+ ROS_ERROR("fcamFindByUrl returned illegal return value. This should never happen and is a bug.");
}
// Configure the camera with its IP address, wait up to 500ms for completion
- camera_ = fcamCamListGetEntry(&camList, index);
- retval = fcamConfigure(camera_, config_.ip_address.c_str(), SEC_TO_USEC(0.5));
+// camera_ = fcamCamListGetEntry(&camList, index);
+ retval = fcamConfigure(&camera_, address_.c_str(), SEC_TO_USEC(0.5));
if (retval != 0) {
if (retval == ERR_CONFIG_ARPFAIL) {
ROS_WARN("Unable to create ARP entry (are you root?), continuing anyway");
@@ -381,27 +405,27 @@
return;
}
}
- ROS_INFO("Configured camera, S/N #%u, IP address %s, name %s",
- camera_->serial, config_.ip_address.c_str(), camera_->cam_name);
+ camera_.serial = camera_.serial;
+ hwinfo_ = camera_.hwinfo;
+
+ ROS_INFO("Configured camera. Complete URLs: serial://%u@%s#%s name://%s@%s#%s",
+ camera_.serial, address_.c_str(), interface_.c_str(), camera_.cam_name, address_.c_str(), interface_.c_str());
- serial_number_ = camera_->serial;
- hwinfo_ = camera_->hwinfo;
-
// We are going to receive the video on this host, so we need our own MAC address
- if ( wgEthGetLocalMac(camera_->ifName, &localMac_) != 0 ) {
- ROS_FATAL("Unable to get local MAC address for interface %s", camera_->ifName);
+ if ( wgEthGetLocalMac(camera_.ifName, &localMac_) != 0 ) {
+ ROS_FATAL("Unable to get local MAC address for interface %s", camera_.ifName);
return;
}
// We also need our local IP address
- if ( wgIpGetLocalAddr(camera_->ifName, &localIp_) != 0) {
- ROS_FATAL("Unable to get local IP address for interface %s", camera_->ifName);
+ if ( wgIpGetLocalAddr(camera_.ifName, &localIp_) != 0) {
+ ROS_FATAL("Unable to get local IP address for interface %s", camera_.ifName);
return;
}
// Select trigger mode.
- if ( fcamTriggerControl( camera_, config_.ext_trig ? TRIG_STATE_EXTERNAL : TRIG_STATE_INTERNAL ) != 0) {
- ROS_FATAL("Trigger mode set error. Is %s accessible from interface %s? (Try running route to check.)", config_.ip_address.c_str(), config_.if_name.c_str());
+ if ( fcamTriggerControl( &camera_, config_.ext_trig ? TRIG_STATE_EXTERNAL : TRIG_STATE_INTERNAL ) != 0) {
+ ROS_FATAL("Trigger mode set error. Is %s accessible from interface %s? (Try running route to check.)", address_.c_str(), interface_.c_str());
return;
}
@@ -431,7 +455,7 @@
}
// Select a video mode
- if ( fcamImagerModeSelect( camera_, video_mode_ ) != 0) {
+ if ( fcamImagerModeSelect( &camera_, video_mode_ ) != 0) {
ROS_FATAL("Mode select error");
//node_handle_.shutdown();
return;
@@ -439,12 +463,12 @@
int bin_size = width_ > 320 ? 1 : 2;
// Set horizontal blanking
- if ( fcamReliableSensorWrite( camera_, MT9V_REG_HORIZONTAL_BLANKING, MT9VModes[video_mode_].hblank * bin_size, NULL ) != 0)
+ if ( fcamReliableSensorWrite( &camera_, MT9V_REG_HORIZONTAL_BLANKING, MT9VModes[video_mode_].hblank * bin_size, NULL ) != 0)
{
ROS_WARN("Error setting horizontal blanking.");
}
- if ( fcamReliableSensorWrite( camera_, MT9V_REG_VERTICAL_BLANKING, MT9VModes[video_mode_].vblank, NULL) != 0)
+ if ( fcamReliableSensorWrite( &camera_, MT9V_REG_VERTICAL_BLANKING, MT9VModes[video_mode_].vblank, NULL) != 0)
{
ROS_WARN("Error setting vertical blanking.");
}
@@ -458,14 +482,14 @@
}
*/
- if (fcamReliableSensorWrite(camera_, MT9V_REG_AGC_AEC_ENABLE, config_.auto_gain * 2 + config_.auto_exposure, NULL) != 0)
+ if (fcamReliableSensorWrite(&camera_, MT9V_REG_AGC_AEC_ENABLE, config_.auto_gain * 2 + config_.auto_exposure, NULL) != 0)
{
ROS_WARN("Error setting AGC/AEC mode. Exposure and gain may be incorrect.");
}
if (!config_.auto_gain)
{
- if ( fcamReliableSensorWrite( camera_, MT9V_REG_ANALOG_GAIN, config_.gain, NULL) != 0)
+ if ( fcamReliableSensorWrite( &camera_, MT9V_REG_ANALOG_GAIN, config_.gain, NULL) != 0)
{
ROS_WARN("Error setting analog gain.");
}
@@ -483,7 +507,7 @@
if (explines > 32767) /// @TODO warning here?
explines = 32767;
ROS_DEBUG("Setting exposure lines to %i.", explines);
- if ( fcamReliableSensorWrite( camera_, MT9V_REG_TOTAL_SHUTTER_WIDTH, explines, NULL) != 0)
+ if ( fcamReliableSensorWrite( &camera_, MT9V_REG_TOTAL_SHUTTER_WIDTH, explines, NULL) != 0)
{
ROS_WARN("Error setting exposure.");
}
@@ -509,7 +533,7 @@
{
ROS_DEBUG("start()");
assert(state_ == OPENED);
- image_thread_.reset(new boost::thread(boost::bind(&ForearmCamDriver::imageThread, this, config_.port)));
+ image_thread_.reset(new boost::thread(boost::bind(&ForearmCamDriver::imageThread, this)));
state_ = RUNNING;
}
@@ -530,7 +554,7 @@
int setTestMode(uint16_t mode, diagnostic_updater::DiagnosticStatusWrapper &status)
{
- if ( fcamReliableSensorWrite( camera_, 0x7F, mode, NULL ) != 0) {
+ if ( fcamReliableSensorWrite( &camera_, 0x7F, mode, NULL ) != 0) {
status.summary(2, "Could not set imager into test mode.");
status.adds("Writing imager test mode", "Fail");
return 1;
@@ -542,7 +566,7 @@
usleep(100000);
uint16_t inmode;
- if ( fcamReliableSensorRead( camera_, 0x7F, &inmode, NULL ) != 0) {
+ if ( fcamReliableSensorRead( &camera_, 0x7F, &inmode, NULL ) != 0) {
status.summary(2, "Could not read imager mode back.");
status.adds("Reading imager test mode", "Fail");
return 1;
@@ -567,11 +591,11 @@
std::string getID()
{
- return (boost::format("FCAM%05u") % serial_number_).str();
+ return (boost::format("FCAM%05u") % camera_.serial).str();
}
private:
- void imageThread(int port)
+ void imageThread()
{
// Start video
if (!trig_controller_cmd_.empty())
@@ -600,9 +624,9 @@
return;
}
// Receive video
- fcamVidReceive(camera_->ifName, port, height_, width_, &ForearmCamDriver::frameHandle...
[truncated message content] |
|
From: <tf...@us...> - 2009-08-04 16:06:37
|
Revision: 20639
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=20639&view=rev
Author: tfoote
Date: 2009-08-04 16:06:28 +0000 (Tue, 04 Aug 2009)
Log Message:
-----------
fixing tests
Modified Paths:
--------------
pkg/trunk/motion_planning/mpglue/test/mpglue_test.cpp
pkg/trunk/stacks/visual_feature_detectors/outlet_detection/test/detect_outlet_from_bag.py
Modified: pkg/trunk/motion_planning/mpglue/test/mpglue_test.cpp
===================================================================
--- pkg/trunk/motion_planning/mpglue/test/mpglue_test.cpp 2009-08-04 15:02:11 UTC (rev 20638)
+++ pkg/trunk/motion_planning/mpglue/test/mpglue_test.cpp 2009-08-04 16:06:28 UTC (rev 20639)
@@ -36,7 +36,7 @@
#include <costmap_2d/costmap_2d.h>
#include <mpglue/costmapper.h>
#include <mpglue/plan.h>
-#include <robot_msgs/Pose.h>
+#include <geometry_msgs/Pose.h>
#include <sfl/util/numeric.hpp>
using namespace std;
@@ -338,10 +338,10 @@
for (double yy(89); yy <= 102; yy += 1.1)
for (double theta(-M_PI / 2); theta <= 2 * M_PI; theta += 0.1) {
mpglue::waypoint_s const wp0(xx, yy, theta);
- robot_msgs::Pose pp0;
+ geometry_msgs::Pose pp0;
wp0.toPose(pp0);
mpglue::waypoint_s const wp1(pp0);
- // robot_msgs::Pose pp1;
+ // geometry_msgs::Pose pp1;
// wp1.toPose(pp1);
double const dx(wp1.x - wp0.x);
double const dy(wp1.y - wp0.y);
Modified: pkg/trunk/stacks/visual_feature_detectors/outlet_detection/test/detect_outlet_from_bag.py
===================================================================
--- pkg/trunk/stacks/visual_feature_detectors/outlet_detection/test/detect_outlet_from_bag.py 2009-08-04 15:02:11 UTC (rev 20638)
+++ pkg/trunk/stacks/visual_feature_detectors/outlet_detection/test/detect_outlet_from_bag.py 2009-08-04 16:06:28 UTC (rev 20639)
@@ -50,7 +50,7 @@
# imports the handle detector service
from outlet_detection.srv import *
-from robot_msgs.msg import PointStamped, PoseStamped, Quaternion
+from geometry_msgs.msg import PointStamped, PoseStamped, Quaternion
class TestOutletDetector(unittest.TestCase):
def setUp(self):
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <vij...@us...> - 2009-08-04 20:59:38
|
Revision: 20694
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=20694&view=rev
Author: vijaypradeep
Date: 2009-08-04 20:59:30 +0000 (Tue, 04 Aug 2009)
Log Message:
-----------
Lots of additions for calibration capture pipeline. Also fixing led_detection build.
Modified Paths:
--------------
pkg/trunk/calibration/sandbox/pr2_calibration_actions/CMakeLists.txt
pkg/trunk/calibration/sandbox/pr2_calibration_actions/manifest.xml
pkg/trunk/vision/led_detection/include/led_detection/led_detector.h
pkg/trunk/vision/led_detection/manifest.xml
pkg/trunk/vision/led_detection/src/led_detector.cpp
pkg/trunk/vision/led_detection/src/led_detector_node.cpp
Added Paths:
-----------
pkg/trunk/calibration/calibration_msgs/msg/ImagePoint.msg
pkg/trunk/calibration/calibration_msgs/msg/ImagePointStamped.msg
pkg/trunk/calibration/sandbox/calibration_message_filters/
pkg/trunk/calibration/sandbox/calibration_message_filters/CMakeLists.txt
pkg/trunk/calibration/sandbox/calibration_message_filters/Makefile
pkg/trunk/calibration/sandbox/calibration_message_filters/include/
pkg/trunk/calibration/sandbox/calibration_message_filters/include/calibration_message_filters/
pkg/trunk/calibration/sandbox/calibration_message_filters/include/calibration_message_filters/deflated.h
pkg/trunk/calibration/sandbox/calibration_message_filters/include/calibration_message_filters/joint_states_deflater.h
pkg/trunk/calibration/sandbox/calibration_message_filters/include/calibration_message_filters/sandwich.h
pkg/trunk/calibration/sandbox/calibration_message_filters/include/calibration_message_filters/sandwich_elem.h
pkg/trunk/calibration/sandbox/calibration_message_filters/manifest.xml
pkg/trunk/calibration/sandbox/calibration_message_filters/test/
pkg/trunk/calibration/sandbox/calibration_message_filters/test/CMakeLists.txt
pkg/trunk/calibration/sandbox/calibration_message_filters/test/sandwich_unittest.cpp
pkg/trunk/calibration/sandbox/pr2_calibration_actions/include/pr2_calibration_actions/capture_hand_led.h
pkg/trunk/calibration/sandbox/pr2_calibration_actions/src/capture_hand_led.cpp
Removed Paths:
-------------
pkg/trunk/calibration/kinematic_calibration/msg/ImagePoint.msg
pkg/trunk/calibration/kinematic_calibration/msg/ImagePointStamped.msg
Copied: pkg/trunk/calibration/calibration_msgs/msg/ImagePoint.msg (from rev 20685, pkg/trunk/calibration/kinematic_calibration/msg/ImagePoint.msg)
===================================================================
--- pkg/trunk/calibration/calibration_msgs/msg/ImagePoint.msg (rev 0)
+++ pkg/trunk/calibration/calibration_msgs/msg/ImagePoint.msg 2009-08-04 20:59:30 UTC (rev 20694)
@@ -0,0 +1,3 @@
+# Stores the xy location of a point in an image in pixel coordinates
+float64 x
+float64 y
Copied: pkg/trunk/calibration/calibration_msgs/msg/ImagePointStamped.msg (from rev 20685, pkg/trunk/calibration/kinematic_calibration/msg/ImagePointStamped.msg)
===================================================================
--- pkg/trunk/calibration/calibration_msgs/msg/ImagePointStamped.msg (rev 0)
+++ pkg/trunk/calibration/calibration_msgs/msg/ImagePointStamped.msg 2009-08-04 20:59:30 UTC (rev 20694)
@@ -0,0 +1,2 @@
+Header header
+ImagePoint image_point
\ No newline at end of file
Deleted: pkg/trunk/calibration/kinematic_calibration/msg/ImagePoint.msg
===================================================================
--- pkg/trunk/calibration/kinematic_calibration/msg/ImagePoint.msg 2009-08-04 20:56:29 UTC (rev 20693)
+++ pkg/trunk/calibration/kinematic_calibration/msg/ImagePoint.msg 2009-08-04 20:59:30 UTC (rev 20694)
@@ -1,3 +0,0 @@
-# Stores the xy location of a point in an image in pixel coordinates
-float64 x
-float64 y
Deleted: pkg/trunk/calibration/kinematic_calibration/msg/ImagePointStamped.msg
===================================================================
--- pkg/trunk/calibration/kinematic_calibration/msg/ImagePointStamped.msg 2009-08-04 20:56:29 UTC (rev 20693)
+++ pkg/trunk/calibration/kinematic_calibration/msg/ImagePointStamped.msg 2009-08-04 20:59:30 UTC (rev 20694)
@@ -1,2 +0,0 @@
-Header header
-ImagePoint image_point
\ No newline at end of file
Added: pkg/trunk/calibration/sandbox/calibration_message_filters/CMakeLists.txt
===================================================================
--- pkg/trunk/calibration/sandbox/calibration_message_filters/CMakeLists.txt (rev 0)
+++ pkg/trunk/calibration/sandbox/calibration_message_filters/CMakeLists.txt 2009-08-04 20:59:30 UTC (rev 20694)
@@ -0,0 +1,10 @@
+cmake_minimum_required(VERSION 2.4.6)
+include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
+rospack(calibration_message_filters)
+
+set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
+
+rospack_add_library(calibration_message_filters src/stationary_checker.cpp
+ src/joint_states_deflater.cpp)
+
+add_subdirectory(test EXCLUDE_FROM_ALL)
Added: pkg/trunk/calibration/sandbox/calibration_message_filters/Makefile
===================================================================
--- pkg/trunk/calibration/sandbox/calibration_message_filters/Makefile (rev 0)
+++ pkg/trunk/calibration/sandbox/calibration_message_filters/Makefile 2009-08-04 20:59:30 UTC (rev 20694)
@@ -0,0 +1 @@
+include $(shell rospack find mk)/cmake.mk
Added: pkg/trunk/calibration/sandbox/calibration_message_filters/include/calibration_message_filters/deflated.h
===================================================================
--- pkg/trunk/calibration/sandbox/calibration_message_filters/include/calibration_message_filters/deflated.h (rev 0)
+++ pkg/trunk/calibration/sandbox/calibration_message_filters/include/calibration_message_filters/deflated.h 2009-08-04 20:59:30 UTC (rev 20694)
@@ -0,0 +1,64 @@
+/*********************************************************************
+* 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
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * 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
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+#ifndef CALIBRATION_MESSAGE_FILTERS_DEFLATED_H_
+#define CALIBRATION_MESSAGE_FILTERS_DEFLATED_H_
+
+#include <boost/shared_ptr.hpp>
+#include "roslib/Header.h"
+
+namespace calibration_message_filters
+{
+
+class Deflated
+{
+public:
+ roslib::Header header; // Need header to put in cache
+ std::vector<double> deflated_;
+};
+
+template <class M>
+class DeflatedMsg : public Deflated
+{
+public:
+ typedef boost::shared_ptr<const M> MConstPtr;
+ MConstPtr msg;
+};
+
+}
+
+#endif
+
+
+
Added: pkg/trunk/calibration/sandbox/calibration_message_filters/include/calibration_message_filters/joint_states_deflater.h
===================================================================
--- pkg/trunk/calibration/sandbox/calibration_message_filters/include/calibration_message_filters/joint_states_deflater.h (rev 0)
+++ pkg/trunk/calibration/sandbox/calibration_message_filters/include/calibration_message_filters/joint_states_deflater.h 2009-08-04 20:59:30 UTC (rev 20694)
@@ -0,0 +1,67 @@
+/*********************************************************************
+* 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
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * 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
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+#ifndef CALIBRATION_MESSAGE_FILTERS_JOINT_STATES_DEFLATER_H_
+#define CALIBRATION_MESSAGE_FILTERS_JOINT_STATES_DEFLATER_H_
+
+#include "calibration_message_filters/deflated.h"
+#include "mechanism_msgs/JointStates.h"
+
+namespace calibration_message_filters
+{
+
+
+class JointStatesDeflater
+{
+public:
+ typedef DeflatedMsg<mechanism_msgs::JointStates> DeflatedJointStates;
+
+ JointStatesDeflater();
+
+ void setDeflationJointNames(std::vector<std::string> joint_names);
+ void deflate(const mechanism_msgs::JointStatesConstPtr& joint_states, DeflatedJointStates& deflated_elem);
+
+private:
+ std::vector<unsigned int> mapping_;
+ std::vector<std::string> joint_names_;
+
+ void updateMapping(const mechanism_msgs::JointStates& joint_states);
+
+};
+
+}
+
+
+
+#endif
Added: pkg/trunk/calibration/sandbox/calibration_message_filters/include/calibration_message_filters/sandwich.h
===================================================================
--- pkg/trunk/calibration/sandbox/calibration_message_filters/include/calibration_message_filters/sandwich.h (rev 0)
+++ pkg/trunk/calibration/sandbox/calibration_message_filters/include/calibration_message_filters/sandwich.h 2009-08-04 20:59:30 UTC (rev 20694)
@@ -0,0 +1,262 @@
+/*********************************************************************
+* 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
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * 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
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+#ifndef CALIBRATION_MESSAGE_FILTERS_SANDWICH_H_
+#define CALIBRATION_MESSAGE_FILTERS_SANDWICH_H_
+
+#include <boost/thread.hpp>
+#include <boost/shared_ptr.hpp>
+#include <boost/signals.hpp>
+
+#include "message_filters/cache.h"
+
+#include "ros/time.h"
+#include "calibration_message_filters/sandwich_elem.h"
+
+namespace calibration_message_filters
+{
+
+template<class M, class S>
+class Sandwich
+{
+public:
+ typedef boost::shared_ptr<M const> MConstPtr;
+ typedef boost::shared_ptr<S const> SConstPtr;
+ typedef boost::function<void(const SandwichElem<M,S>&)> Callback;
+ typedef boost::signal<void(const SandwichElem<M,S>&)> Signal;
+
+ template<class F1>
+ Sandwich(F1& f1, message_filters::Cache<S>& cache, unsigned int queue_size=1,
+ const ros::Duration& padding_before=ros::Duration(0,0),
+ const ros::Duration& padding_after=ros::Duration(0,0),
+ const ros::Duration& expiration_limit=ros::Duration(10,0) )
+ {
+ setQueueSize(queue_size);
+ setPadding(padding_before, padding_after);
+ setExpirationLimit(expiration_limit);
+ connectTo(f1, cache);
+ }
+
+ Sandwich(unsigned int queue_size=1,
+ const ros::Duration& padding_before=ros::Duration(0,0),
+ const ros::Duration& padding_after=ros::Duration(0,0),
+ const ros::Duration& expiration_limit=ros::Duration(10,0) )
+ {
+ cache_ = NULL;
+ setQueueSize(queue_size);
+ setPadding(padding_before, padding_after);
+ setExpirationLimit(expiration_limit);
+ }
+
+ template<class F1>
+ void connectInput(F1& f1, message_filters::Cache<S>& cache)
+ {
+ {
+ boost::mutex::scoped_lock lock(signal_mutex_);
+ incoming_msg_connection_ = f1.connect(boost::bind(&Sandwich::addToQueue, this, _1));
+ incoming_gate_connection_ = cache.connect(boost::bind(&Sandwich::addToGate, this, _1));
+ }
+
+ {
+ boost::mutex::scoped_lock lock(data_mutex_);
+ cache_ = &cache;
+ }
+ }
+
+ void connectCacheInput(message_filters::Cache<S>& cache)
+ {
+ {
+ boost::mutex::scoped_lock lock(signal_mutex_);
+ incoming_gate_connection_ = cache.registerCallback(boost::bind(&Sandwich::addToGate, this, _1));
+ }
+
+ {
+ boost::mutex::scoped_lock lock(data_mutex_);
+ cache_ = &cache;
+ }
+ }
+
+ message_filters::Connection registerCallback(const Callback& callback)
+ {
+ boost::mutex::scoped_lock lock(signal_mutex_);
+ return message_filters::Connection(boost::bind(&Sandwich::disconnect, this, _1), signal_.connect(callback));
+ }
+
+ ~Sandwich()
+ {
+ incoming_msg_connection_.disconnect();
+ incoming_gate_connection_.disconnect();
+ }
+
+ void setQueueSize(unsigned int queue_size)
+ {
+ boost::mutex::scoped_lock lock(data_mutex_);
+
+ if (queue_size == 0)
+ return;
+
+ queue_size_ = queue_size;
+ }
+
+ void setPadding(const ros::Duration& padding_before,
+ const ros::Duration& padding_after)
+ {
+ padding_before_ = padding_before;
+ padding_after_ = padding_after;
+ }
+
+ void setExpirationLimit(const ros::Duration& expiration_limit)
+ {
+ boost::mutex::scoped_lock lock(data_mutex_);
+ if (expiration_limit < ros::Duration(0,0))
+ return;
+ expiration_limit_ = expiration_limit;
+ }
+
+ void addToQueue(const MConstPtr& msg)
+ {
+
+ {
+ boost::mutex::scoped_lock lock(data_mutex_);
+
+ while (queue_.size() >= queue_size_) // Keep popping off old data until we have space for a new msg
+ queue_.pop_front() ; // The front of the deque has the oldest elem, so we can get rid of it
+
+ // Add the msg to the queue, sorted by header.stamp
+ typename std::deque<MConstPtr >::reverse_iterator rev_it = queue_.rbegin();
+ // Keep walking backwards along deque until we hit the beginning,
+ // or until we find a timestamp that's smaller than (or equal to) msg's timestamp
+ while(rev_it != queue_.rend() && (*rev_it)->header.stamp > msg->header.stamp)
+ rev_it++;
+ // Add msg to the cache
+ queue_.insert(rev_it.base(), msg);
+
+ }
+
+ processQueue();
+ }
+
+ void addToGate(const SConstPtr& msg)
+ {
+ processQueue();
+ }
+
+private:
+ void disconnect(const message_filters::Connection& c)
+ {
+ boost::mutex::scoped_lock lock(signal_mutex_);
+ signal_.disconnect(c.getBoostConnection());
+ }
+
+ void processQueue()
+ {
+ std::vector<SandwichElem<M,S> > valid_elems;
+ {
+
+ boost::mutex::scoped_lock lock(data_mutex_);
+
+ if (!cache_)
+ return;
+
+ // Process all elems in the queue
+ typename std::deque<MConstPtr >::iterator it = queue_.begin();
+
+ while (it != queue_.end())
+ {
+ // Check if we need to throw away the elem
+ if ( (*it)->header.stamp - padding_before_ < cache_->getLatestTime() - expiration_limit_ )
+ {
+ it = queue_.erase(it);
+ }
+ else
+ {
+ // See what the interval looks like
+ std::vector<SConstPtr > interval_msgs = cache_->getSurroundingInterval((*it)->header.stamp - padding_before_,
+ (*it)->header.stamp + padding_after_);
+ if (interval_msgs.size() < 2)
+ {
+ ++it;
+ }
+ else if((*it)->header.stamp + padding_after_ < interval_msgs.back()->header.stamp &&
+ (*it)->header.stamp - padding_before_ > interval_msgs.front()->header.stamp)
+ {
+ SandwichElem<M,S> sandwich_elem;
+ sandwich_elem.data = *it;
+ sandwich_elem.interval = interval_msgs;
+ it = queue_.erase(it);
+ valid_elems.push_back(sandwich_elem);
+ }
+ else
+ {
+ ++it;
+ }
+ }
+ }
+ }
+
+ // Call the callback for each elem that was within range
+ {
+ boost::mutex::scoped_lock lock(signal_mutex_);
+ for (unsigned int i=0; i<valid_elems.size(); i++)
+ signal_(valid_elems[i]) ;
+ }
+
+ }
+
+ boost::mutex signal_mutex_;
+ message_filters::Connection incoming_msg_connection_;
+ message_filters::Connection incoming_gate_connection_;
+ Signal signal_;
+
+ boost::mutex data_mutex_;
+ std::deque<MConstPtr > queue_;
+ message_filters::Cache<S>* cache_;
+ ros::Duration expiration_limit_;
+ unsigned int queue_size_;
+ ros::Duration padding_before_;
+ ros::Duration padding_after_;
+
+} ;
+
+
+
+
+
+
+
+
+}
+
+
+#endif
Added: pkg/trunk/calibration/sandbox/calibration_message_filters/include/calibration_message_filters/sandwich_elem.h
===================================================================
--- pkg/trunk/calibration/sandbox/calibration_message_filters/include/calibration_message_filters/sandwich_elem.h (rev 0)
+++ pkg/trunk/calibration/sandbox/calibration_message_filters/include/calibration_message_filters/sandwich_elem.h 2009-08-04 20:59:30 UTC (rev 20694)
@@ -0,0 +1,60 @@
+/*********************************************************************
+* 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
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * 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
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+#ifndef CALIBRATION_MESSAGE_FILTERS_SANDWICH_ELEM_H_
+#define CALIBRATION_MESSAGE_FILTERS_SANDWICH_ELEM_H_
+
+namespace calibration_message_filters
+{
+
+#include <vector>
+#include "boost/shared_ptr.hpp"
+
+/**
+ * Output type for the sandwich filter. Stores some message type M,
+ * along with a vector of message type S, which 'sandwiches' M. Thus,
+ * the vector of S messages surrounds the message M, in time.
+ */
+
+template <class M, class S>
+struct SandwichElem
+{
+ boost::shared_ptr<const M> data;
+ std::vector< boost::shared_ptr<const S> > interval;
+};
+
+}
+
+
+#endif // CALIBRATION_MESSAGE_FILTERS_SANDWICH_ELEM_H_
Added: pkg/trunk/calibration/sandbox/calibration_message_filters/manifest.xml
===================================================================
--- pkg/trunk/calibration/sandbox/calibration_message_filters/manifest.xml (rev 0)
+++ pkg/trunk/calibration/sandbox/calibration_message_filters/manifest.xml 2009-08-04 20:59:30 UTC (rev 20694)
@@ -0,0 +1,14 @@
+<package>
+ <description>Message filters that are especially useful for performing robot calibration tasks</description>
+ <author>Vijay Pradeep (vpr...@wi...)</author>
+ <license>BSD</license>
+ <review status="unreviewed" notes=""/>
+ <depend package="roscpp" />
+ <depend package="message_filters" />
+ <depend package="mechanism_msgs" />
+
+ <export>
+ <cpp cflags="-I${prefix}/include" lflags="-L${prefix}/lib -lcalibration_message_filters" />
+ </export>
+ <url>http://pr.willowgarage.com/wiki/message_filters</url>
+</package>
Added: pkg/trunk/calibration/sandbox/calibration_message_filters/test/CMakeLists.txt
===================================================================
--- pkg/trunk/calibration/sandbox/calibration_message_filters/test/CMakeLists.txt (rev 0)
+++ pkg/trunk/calibration/sandbox/calibration_message_filters/test/CMakeLists.txt 2009-08-04 20:59:30 UTC (rev 20694)
@@ -0,0 +1,7 @@
+set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR})
+
+# ********** Tests **********
+rospack_add_gtest(test/sandwich_unittest sandwich_unittest.cpp)
+
+rospack_add_gtest(test/joint_states_deflater_unittest joint_states_deflater_unittest.cpp)
+target_link_libraries(test/joint_states_deflater_unittest calibration_message_filters)
\ No newline at end of file
Added: pkg/trunk/calibration/sandbox/calibration_message_filters/test/sandwich_unittest.cpp
===================================================================
--- pkg/trunk/calibration/sandbox/calibration_message_filters/test/sandwich_unittest.cpp (rev 0)
+++ pkg/trunk/calibration/sandbox/calibration_message_filters/test/sandwich_unittest.cpp 2009-08-04 20:59:30 UTC (rev 20694)
@@ -0,0 +1,138 @@
+/*********************************************************************
+* 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
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * 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
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+#include <gtest/gtest.h>
+
+#include "ros/time.h"
+#include "message_filters/cache.h"
+#include "calibration_message_filters/sandwich.h"
+
+using namespace std ;
+using namespace message_filters ;
+using namespace calibration_message_filters ;
+
+struct Header
+{
+ ros::Time stamp ;
+} ;
+
+
+
+struct Msg
+{
+ Header header;
+ int data;
+} ;
+
+
+
+struct GateMsg
+{
+ Header header;
+ int junk1;
+ double junk2;
+} ;
+
+typedef boost::shared_ptr<const Msg> MsgConstPtr;
+typedef boost::shared_ptr<const GateMsg> GateMsgConstPtr;
+
+MsgConstPtr buildMsg(ros::Time time, int data)
+{
+ boost::shared_ptr<Msg> msg_ptr(new Msg);
+ msg_ptr->header.stamp = time;
+ msg_ptr->data = data;
+ return msg_ptr;
+}
+
+GateMsgConstPtr buildGateMsg(ros::Time time)
+{
+ boost::shared_ptr<GateMsg> msg_ptr(new GateMsg);
+ msg_ptr->header.stamp = time;
+ return msg_ptr;
+}
+
+class Helper
+{
+public:
+ Helper()
+ : count_(0)
+ {}
+
+ void cb(const SandwichElem<Msg,GateMsg>& sandwich_elem)
+ {
+ ++count_;
+ latest_elem_ = sandwich_elem;
+ }
+
+ int32_t count_;
+ SandwichElem<Msg,GateMsg> latest_elem_;
+};
+
+TEST(Sandwich, easyNoPadding)
+{
+ Cache<GateMsg> cache(10);
+ Sandwich<Msg, GateMsg> sandwich;
+ Helper h;
+ //cache.connect(boost::bind(&Sandwich<Msg,GateMsg>::addToGate, &sandwich, _1));
+ sandwich.connectCacheInput(cache);
+ sandwich.registerCallback(boost::bind(&Helper::cb, &h, _1));
+
+ cache.add(buildGateMsg(ros::Time(10,0)));
+ EXPECT_EQ(0, h.count_);
+ cache.add(buildGateMsg(ros::Time(20,0)));
+ EXPECT_EQ(0, h.count_);
+ sandwich.addToQueue(buildMsg(ros::Time(15,0),1));
+ EXPECT_EQ(1, h.count_);
+ EXPECT_EQ((unsigned int) 2, h.latest_elem_.interval.size());
+ EXPECT_EQ(h.latest_elem_.interval[0]->header.stamp, ros::Time(10,0));
+ EXPECT_EQ(h.latest_elem_.interval[1]->header.stamp, ros::Time(20,0));
+
+ sandwich.addToQueue(buildMsg(ros::Time(25,0),2));
+ EXPECT_EQ(1, h.count_);
+ cache.add(buildGateMsg(ros::Time(30,0)));
+ EXPECT_EQ(2, h.count_);
+ EXPECT_EQ((unsigned int) 2, h.latest_elem_.interval.size());
+ EXPECT_EQ(h.latest_elem_.interval[0]->header.stamp, ros::Time(20,0));
+ EXPECT_EQ(h.latest_elem_.interval[1]->header.stamp, ros::Time(30,0));
+
+ sandwich.addToQueue(buildMsg(ros::Time(5,0),2));
+ EXPECT_EQ(2, h.count_);
+}
+
+
+int main(int argc, char **argv){
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
+
Modified: pkg/trunk/calibration/sandbox/pr2_calibration_actions/CMakeLists.txt
===================================================================
--- pkg/trunk/calibration/sandbox/pr2_calibration_actions/CMakeLists.txt 2009-08-04 20:56:29 UTC (rev 20693)
+++ pkg/trunk/calibration/sandbox/pr2_calibration_actions/CMakeLists.txt 2009-08-04 20:59:30 UTC (rev 20694)
@@ -5,13 +5,15 @@
genmsg()
-rospack_add_library(${PROJECT_NAME} src/stereo_cb_action.cpp)
+rospack_add_library(${PROJECT_NAME} #src/stereo_cb_action.cpp
+ src/capture_hand_led.cpp)
+
rospack_add_boost_directories()
rospack_link_boost(${PROJECT_NAME} thread)
-rospack_add_executable(run_action_stereo_cb src/run_action_stereo_cb.cpp)
-target_link_libraries(run_action_stereo_cb ${PROJECT...
[truncated message content] |
|
From: <bla...@us...> - 2009-08-04 21:50:53
|
Revision: 20705
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=20705&view=rev
Author: blaisegassend
Date: 2009-08-04 21:50:43 +0000 (Tue, 04 Aug 2009)
Log Message:
-----------
Increased led_test tolerance to 2. Allowed self test to retry open and start commands before failing.
Modified Paths:
--------------
pkg/trunk/pr2/qualification/tests/forearm_cam_test/led_flash_test.launch
pkg/trunk/stacks/driver_common/driver_base/include/driver_base/driver_node.h
Modified: pkg/trunk/pr2/qualification/tests/forearm_cam_test/led_flash_test.launch
===================================================================
--- pkg/trunk/pr2/qualification/tests/forearm_cam_test/led_flash_test.launch 2009-08-04 21:46:10 UTC (rev 20704)
+++ pkg/trunk/pr2/qualification/tests/forearm_cam_test/led_flash_test.launch 2009-08-04 21:50:43 UTC (rev 20705)
@@ -24,7 +24,7 @@
<param name="min_high" type="double" value="-0.009"/>
<param name="max_high" type="double" value="-0.004"/>
<param name="skip" type="int" value="10"/>
- <param name="tolerance" type="int" value="1" />
+ <param name="tolerance" type="int" value="2" />
<param name="frames" type="int" value="300"/>
<remap from="image" to="/forearm/image_raw"/>
</node>
Modified: pkg/trunk/stacks/driver_common/driver_base/include/driver_base/driver_node.h
===================================================================
--- pkg/trunk/stacks/driver_common/driver_base/include/driver_base/driver_node.h 2009-08-04 21:46:10 UTC (rev 20704)
+++ pkg/trunk/stacks/driver_common/driver_base/include/driver_base/driver_node.h 2009-08-04 21:50:43 UTC (rev 20705)
@@ -249,6 +249,12 @@
{
driver_.open();
+ if (!driver_.isOpened()) // Retry to avoid occasional glitches.
+ {
+ sleep(2);
+ driver_.open();
+ }
+
if (driver_.isOpened())
status.summaryf(0, "Successfully opened device %s", driver_.getID().c_str());
else
@@ -259,6 +265,12 @@
{
driver_.start();
+ if (!driver_.isRunning()) // Retry to avoid occasional glitches.
+ {
+ sleep(2);
+ driver_.start();
+ }
+
if (driver_.isRunning())
status.summaryf(0, "Successfully started streaming.");
else
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <stu...@us...> - 2009-08-04 21:52:04
|
Revision: 20707
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=20707&view=rev
Author: stuglaser
Date: 2009-08-04 21:51:56 +0000 (Tue, 04 Aug 2009)
Log Message:
-----------
Updated tuckarm.py for the controller spawning changes.
Modified Paths:
--------------
pkg/trunk/controllers/pr2_mechanism_controllers/src/joint_trajectory_controller.cpp
pkg/trunk/stacks/pr2/pr2_default_controllers/l_arm_joint_trajectory_controller.xml
pkg/trunk/stacks/pr2/pr2_default_controllers/r_arm_joint_trajectory_controller.xml
pkg/trunk/stacks/pr2/pr2_default_controllers/scripts/tuckarm.py
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/joint_trajectory_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/joint_trajectory_controller.cpp 2009-08-04 21:51:48 UTC (rev 20706)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/joint_trajectory_controller.cpp 2009-08-04 21:51:56 UTC (rev 20707)
@@ -88,9 +88,15 @@
TiXmlDocument xml_doc;
xml_doc.Parse(xml_string.c_str());
+ if (xml_doc.Error())
+ {
+ ROS_ERROR("Error when parsing XML: %d (%d,%d) %s\n%s", xml_doc.ErrorId(),
+ xml_doc.ErrorRow(), xml_doc.ErrorCol(), xml_doc.ErrorDesc(), xml_string.c_str());
+ return false;
+ }
TiXmlElement *xml = xml_doc.FirstChildElement("controller");
if (!xml){
- ROS_ERROR("could not parse xml %s",xml_string.c_str());
+ ROS_ERROR("could not parse xml: %s\n %s", xml_doc.ErrorDesc(),xml_string.c_str());
return false;
}
Modified: pkg/trunk/stacks/pr2/pr2_default_controllers/l_arm_joint_trajectory_controller.xml
===================================================================
--- pkg/trunk/stacks/pr2/pr2_default_controllers/l_arm_joint_trajectory_controller.xml 2009-08-04 21:51:48 UTC (rev 20706)
+++ pkg/trunk/stacks/pr2/pr2_default_controllers/l_arm_joint_trajectory_controller.xml 2009-08-04 21:51:56 UTC (rev 20707)
@@ -1,42 +1,40 @@
<?xml version="1.0"?>
-<controllers>
- <controller name="l_arm_joint_trajectory_controller" type="JointTrajectoryController">
- <controller name="l_shoulder_pan_controller" topic="l_shoulder_pan_controller" type="JointPDController">
- <joint name="l_shoulder_pan_joint" >
- <pid p="140.0" d="30.0" i="0.8" iClamp="100.0" />
- </joint>
- </controller>
- <controller name="l_shoulder_lift_controller" topic="l_shoulder_pitch_controller" type="JointPDController">
- <joint name="l_shoulder_lift_joint" >
- <pid p="70.0" d="20.0" i="0.0" iClamp="100.0" />
- </joint>
- </controller>
- <controller name="l_upperarm_roll_controller" topic="l_upperarm_roll_controller" type="JointPDController">
- <joint name="l_upper_arm_roll_joint" >
- <pid p="50.0" d="5.0" i="0.2" iClamp="100.0" />
- </joint>
- </controller>
- <controller name="l_elbow_controller" topic="l_elbow_flex_controller" type="JointPDController">
- <joint name="l_elbow_flex_joint" >
- <pid p="100.0" d="5.0" i="0.5" iClamp="100.0" />
- </joint>
- </controller>
- <controller name="l_forearm_roll_controller" topic="l_forearm_roll_controller" type="JointPDController">
- <joint name="l_forearm_roll_joint" >
- <pid p="10.0" d="2.0" i="0.1" iClamp="10.0" />
- </joint>
- </controller>
- <controller name="l_wrist_flex_controller" type="JointPDController">
- <joint name="l_wrist_flex_joint">
- <pid p="10" i="0.1" d="0.5" iClamp="10.0" />
- </joint>
- </controller>
- <controller name="l_wrist_roll_controller" type="JointPDController">
- <joint name="l_wrist_roll_joint">
- <pid p="10" i="0.1" d="0.5" iClamp="10.0" />
- </joint>
- </controller>
- <trajectory interpolation="cubic" />
+<controller name="l_arm_joint_trajectory_controller" type="JointTrajectoryController">
+ <controller name="l_shoulder_pan_controller" topic="l_shoulder_pan_controller" type="JointPDController">
+ <joint name="l_shoulder_pan_joint" >
+ <pid p="140.0" d="30.0" i="0.8" iClamp="100.0" />
+ </joint>
</controller>
-</controllers>
+ <controller name="l_shoulder_lift_controller" topic="l_shoulder_pitch_controller" type="JointPDController">
+ <joint name="l_shoulder_lift_joint" >
+ <pid p="70.0" d="20.0" i="0.0" iClamp="100.0" />
+ </joint>
+ </controller>
+ <controller name="l_upperarm_roll_controller" topic="l_upperarm_roll_controller" type="JointPDController">
+ <joint name="l_upper_arm_roll_joint" >
+ <pid p="50.0" d="5.0" i="0.2" iClamp="100.0" />
+ </joint>
+ </controller>
+ <controller name="l_elbow_controller" topic="l_elbow_flex_controller" type="JointPDController">
+ <joint name="l_elbow_flex_joint" >
+ <pid p="100.0" d="5.0" i="0.5" iClamp="100.0" />
+ </joint>
+ </controller>
+ <controller name="l_forearm_roll_controller" topic="l_forearm_roll_controller" type="JointPDController">
+ <joint name="l_forearm_roll_joint" >
+ <pid p="10.0" d="2.0" i="0.1" iClamp="10.0" />
+ </joint>
+ </controller>
+ <controller name="l_wrist_flex_controller" type="JointPDController">
+ <joint name="l_wrist_flex_joint">
+ <pid p="10" i="0.1" d="0.5" iClamp="10.0" />
+ </joint>
+ </controller>
+ <controller name="l_wrist_roll_controller" type="JointPDController">
+ <joint name="l_wrist_roll_joint">
+ <pid p="10" i="0.1" d="0.5" iClamp="10.0" />
+ </joint>
+ </controller>
+ <trajectory interpolation="cubic" />
+</controller>
Modified: pkg/trunk/stacks/pr2/pr2_default_controllers/r_arm_joint_trajectory_controller.xml
===================================================================
--- pkg/trunk/stacks/pr2/pr2_default_controllers/r_arm_joint_trajectory_controller.xml 2009-08-04 21:51:48 UTC (rev 20706)
+++ pkg/trunk/stacks/pr2/pr2_default_controllers/r_arm_joint_trajectory_controller.xml 2009-08-04 21:51:56 UTC (rev 20707)
@@ -1,42 +1,40 @@
<?xml version="1.0"?>
-<controllers>
- <controller name="r_arm_joint_trajectory_controller" type="JointTrajectoryController">
- <controller name="r_shoulder_pan_controller" topic="r_shoulder_pan_controller" type="JointPDController">
- <joint name="r_shoulder_pan_joint" >
- <pid p="140.0" d="30.0" i="100.0" iClamp="10000.0" />
- </joint>
- </controller>
- <controller name="r_shoulder_lift_controller" topic="r_shoulder_pitch_controller" type="JointPDController">
- <joint name="r_shoulder_lift_joint" >
- <pid p="70.0" d="20.0" i="100.0" iClamp="10000.0" />
- </joint>
- </controller>
- <controller name="r_upperarm_roll_controller" topic="r_upperarm_roll_controller" type="JointPDController">
- <joint name="r_upper_arm_roll_joint" >
- <pid p="50.0" d="5.0" i="100.0" iClamp="10000.0" />
- </joint>
- </controller>
- <controller name="r_elbow_controller" topic="r_elbow_flex_controller" type="JointPDController">
- <joint name="r_elbow_flex_joint" >
- <pid p="100.0" d="5.0" i="100.0" iClamp="10000.0" />
- </joint>
- </controller>
- <controller name="r_forearm_roll_controller" topic="r_forearm_roll_controller" type="JointPDController">
- <joint name="r_forearm_roll_joint" >
- <pid p="10.0" d="2.0" i="100.0" iClamp="10000.0" />
- </joint>
- </controller>
- <controller name="r_wrist_flex_controller" type="JointPDController">
- <joint name="r_wrist_flex_joint">
- <pid p="20.0" i="100.0" d="0.5" iClamp="10000.0" />
- </joint>
- </controller>
- <controller name="r_wrist_roll_controller" type="JointPDController">
- <joint name="r_wrist_roll_joint">
- <pid p="10" i="100.0" d="0.5" iClamp="10000.0" />
- </joint>
- </controller>
- <trajectory interpolation="cubic" />
+<controller name="r_arm_joint_trajectory_controller" type="JointTrajectoryController">
+ <controller name="r_shoulder_pan_controller" topic="r_shoulder_pan_controller" type="JointPDController">
+ <joint name="r_shoulder_pan_joint" >
+ <pid p="140.0" d="30.0" i="100.0" iClamp="10000.0" />
+ </joint>
</controller>
-</controllers>
+ <controller name="r_shoulder_lift_controller" topic="r_shoulder_pitch_controller" type="JointPDController">
+ <joint name="r_shoulder_lift_joint" >
+ <pid p="70.0" d="20.0" i="100.0" iClamp="10000.0" />
+ </joint>
+ </controller>
+ <controller name="r_upperarm_roll_controller" topic="r_upperarm_roll_controller" type="JointPDController">
+ <joint name="r_upper_arm_roll_joint" >
+ <pid p="50.0" d="5.0" i="100.0" iClamp="10000.0" />
+ </joint>
+ </controller>
+ <controller name="r_elbow_controller" topic="r_elbow_flex_controller" type="JointPDController">
+ <joint name="r_elbow_flex_joint" >
+ <pid p="100.0" d="5.0" i="100.0" iClamp="10000.0" />
+ </joint>
+ </controller>
+ <controller name="r_forearm_roll_controller" topic="r_forearm_roll_controller" type="JointPDController">
+ <joint name="r_forearm_roll_joint" >
+ <pid p="10.0" d="2.0" i="100.0" iClamp="10000.0" />
+ </joint>
+ </controller>
+ <controller name="r_wrist_flex_controller" type="JointPDController">
+ <joint name="r_wrist_flex_joint">
+ <pid p="20.0" i="100.0" d="0.5" iClamp="10000.0" />
+ </joint>
+ </controller>
+ <controller name="r_wrist_roll_controller" type="JointPDController">
+ <joint name="r_wrist_roll_joint">
+ <pid p="10" i="100.0" d="0.5" iClamp="10000.0" />
+ </joint>
+ </controller>
+ <trajectory interpolation="cubic" />
+</controller>
Modified: pkg/trunk/stacks/pr2/pr2_default_controllers/scripts/tuckarm.py
===================================================================
--- pkg/trunk/stacks/pr2/pr2_default_controllers/scripts/tuckarm.py 2009-08-04 21:51:48 UTC (rev 20706)
+++ pkg/trunk/stacks/pr2/pr2_default_controllers/scripts/tuckarm.py 2009-08-04 21:51:56 UTC (rev 20707)
@@ -64,6 +64,7 @@
USAGE = 'tuckarm.py <arms> ; <arms> is \'(r)ight\', \'(l)eft\', or \'(b)oth\' arms'
def set_params_right():
+ rospy.set_param("r_arm_joint_trajectory_controller/type", "JointTrajectoryController")
rospy.set_param("r_arm_joint_trajectory_controller/velocity_scaling_factor", 0.5)
rospy.set_param("r_arm_joint_trajectory_controller/trajectory_wait_timeout", 0.25)
@@ -76,6 +77,7 @@
rospy.set_param("r_arm_joint_trajectory_controller/r_wrist_roll_joint/goal_reached_threshold", 0.1)
def set_params_left():
+ rospy.set_param("l_arm_joint_trajectory_controller/type", "JointTrajectoryController")
rospy.set_param("l_arm_joint_trajectory_controller/velocity_scaling_factor", 0.5)
rospy.set_param("l_arm_joint_trajectory_controller/trajectory_wait_timeout", 0.25)
@@ -103,13 +105,16 @@
xml_for_left = open(os.path.join(path, 'l_arm_joint_trajectory_controller.xml'))
xml_for_right = open(os.path.join(path, 'r_arm_joint_trajectory_controller.xml'))
-
+ set_params_left()
+ rospy.set_param('l_arm_joint_trajectory_controller/xml', xml_for_left.read())
+ set_params_right()
+ rospy.set_param('r_arm_joint_trajectory_controller/xml', xml_for_right.read())
+
controllers = []
try:
if side == 'l' or side == 'left':
# tuck traj for left arm
- set_params_left()
- mechanism.spawn_controller(xml_for_left.read(), 1)
+ mechanism.spawn_controller('l_arm_joint_trajectory_controller', True)
controllers.append('l_arm_joint_trajectory_controller')
positions = [[0.4,0.0,0.0,-2.25,0.0,0.0,0.0], [0.0,1.57,1.57,-2.25,0.0,0.0,0.0]]
@@ -119,8 +124,7 @@
elif side == 'r' or side == 'right':
# tuck traj for right arm
- set_params_right()
- resp = mechanism.spawn_controller(xml_for_right.read(), 1)
+ mechanism.spawn_controller('r_arm_joint_trajectory_controller', True)
controllers.append('r_arm_joint_trajectory_controller')
positions = [[-0.4,0.0,0.0,-1.57,0.0,0.0,0.0], [0.0,1.57,-1.57,-1.57,0.0,0.0,0.0]]
go('r', positions)
@@ -130,12 +134,9 @@
elif side == 'b' or side == 'both':
# Both arms
# Holds left arm up at shoulder lift
- set_params_left()
- resp = mechanism.spawn_controller(xml_for_left.read(), 1)
+ mechanism.spawn_controller('r_arm_joint_trajectory_controller', True)
+ mechanism.spawn_controller('l_arm_joint_trajectory_controller', True)
- set_params_right()
- resp = mechanism.spawn_controller(xml_for_right.read(), 1)
-
controllers.append('r_arm_joint_trajectory_controller')
controllers.append('l_arm_joint_trajectory_controller')
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <stu...@us...> - 2009-08-04 22:37:15
|
Revision: 20721
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=20721&view=rev
Author: stuglaser
Date: 2009-08-04 22:37:08 +0000 (Tue, 04 Aug 2009)
Log Message:
-----------
Removing SetPoseStamped from robot_srvs. #1950
Modified Paths:
--------------
pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_hybrid_controller.h
pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_hybrid_controller.cpp
pkg/trunk/highlevel/plugs/plugs_core/include/plugs_core/action_localize_plug_in_gripper.h
pkg/trunk/highlevel/plugs/plugs_core/src/action_localize_plug_in_gripper.cpp
pkg/trunk/sandbox/experimental_controllers/CMakeLists.txt
Added Paths:
-----------
pkg/trunk/controllers/robot_mechanism_controllers/srv/SetPoseStamped.srv
Removed Paths:
-------------
pkg/trunk/stacks/common_msgs/robot_srvs/srv/SetPoseStamped.srv
Modified: pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_hybrid_controller.h
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_hybrid_controller.h 2009-08-04 22:36:32 UTC (rev 20720)
+++ pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_hybrid_controller.h 2009-08-04 22:37:08 UTC (rev 20721)
@@ -43,7 +43,7 @@
#include "filters/filter_chain.h"
#include "control_toolbox/pid_gains_setter.h"
-#include "robot_srvs/SetPoseStamped.h"
+#include "robot_mechanism_controllers/SetPoseStamped.h"
#include "manipulation_msgs/TaskFrameFormalism.h"
#include "robot_mechanism_controllers/CartesianHybridState.h"
@@ -113,8 +113,8 @@
void command(const tf::MessageNotifier<manipulation_msgs::TaskFrameFormalism>::MessagePtr& tff_msg);
- bool setToolFrame(robot_srvs::SetPoseStamped::Request &req,
- robot_srvs::SetPoseStamped::Response &resp);
+ bool setToolFrame(robot_mechanism_controllers::SetPoseStamped::Request &req,
+ robot_mechanism_controllers::SetPoseStamped::Response &resp);
private:
ros::NodeHandle node_;
Modified: pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_hybrid_controller.cpp
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_hybrid_controller.cpp 2009-08-04 22:36:32 UTC (rev 20720)
+++ pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_hybrid_controller.cpp 2009-08-04 22:37:08 UTC (rev 20721)
@@ -639,8 +639,8 @@
}
bool CartesianHybridControllerNode::setToolFrame(
- robot_srvs::SetPoseStamped::Request &req,
- robot_srvs::SetPoseStamped::Response &resp)
+ robot_mechanism_controllers::SetPoseStamped::Request &req,
+ robot_mechanism_controllers::SetPoseStamped::Response &resp)
{
if (!TF.canTransform(c_.kdl_chain_.getSegment(c_.kdl_chain_.getNrOfSegments()-1).getName(), req.p.header.frame_id,
req.p.header.stamp, ros::Duration(3.0)))
Copied: pkg/trunk/controllers/robot_mechanism_controllers/srv/SetPoseStamped.srv (from rev 20720, pkg/trunk/stacks/common_msgs/robot_srvs/srv/SetPoseStamped.srv)
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/srv/SetPoseStamped.srv (rev 0)
+++ pkg/trunk/controllers/robot_mechanism_controllers/srv/SetPoseStamped.srv 2009-08-04 22:37:08 UTC (rev 20721)
@@ -0,0 +1,2 @@
+geometry_msgs/PoseStamped p
+---
Modified: pkg/trunk/highlevel/plugs/plugs_core/include/plugs_core/action_localize_plug_in_gripper.h
===================================================================
--- pkg/trunk/highlevel/plugs/plugs_core/include/plugs_core/action_localize_plug_in_gripper.h 2009-08-04 22:36:32 UTC (rev 20720)
+++ pkg/trunk/highlevel/plugs/plugs_core/include/plugs_core/action_localize_plug_in_gripper.h 2009-08-04 22:37:08 UTC (rev 20721)
@@ -46,9 +46,9 @@
#include <std_msgs/Empty.h>
#include <std_msgs/Float64.h>
-// Srvs
+// Srvs
#include <deprecated_srvs/MoveToPose.h>
-#include "robot_srvs/SetPoseStamped.h"
+#include "robot_mechanism_controllers/SetPoseStamped.h"
//TF
#include <tf/tf.h>
#include <tf/transform_listener.h>
@@ -74,10 +74,10 @@
virtual robot_actions::ResultStatus execute(const std_msgs::Empty& empty, std_msgs::Empty&);
private:
-
+
void reset();
void moveToStage();
- void setToolFrame();
+ void setToolFrame();
std::string action_name_;
@@ -85,7 +85,7 @@
std::string arm_controller_;
std::string servoing_controller_;
-
+
PlugTracker::PlugTracker* detector_;
tf::TransformListener TF;
@@ -94,11 +94,11 @@
geometry_msgs::PoseStamped outlet_pose_;
geometry_msgs::PoseStamped plug_pose_msg_;
tf::Stamped<tf::Pose> plug_pose_;
-
-
+
+
deprecated_srvs::MoveToPose::Request req_pose_;
deprecated_srvs::MoveToPose::Response res_pose_;
-
+
};
}
Modified: pkg/trunk/highlevel/plugs/plugs_core/src/action_localize_plug_in_gripper.cpp
===================================================================
--- pkg/trunk/highlevel/plugs/plugs_core/src/action_localize_plug_in_gripper.cpp 2009-08-04 22:36:32 UTC (rev 20720)
+++ pkg/trunk/highlevel/plugs/plugs_core/src/action_localize_plug_in_gripper.cpp 2009-08-04 22:37:08 UTC (rev 20721)
@@ -204,9 +204,9 @@
node_.unsubscribe("/plug_detector/plug_pose");
return;
}
- robot_srvs::SetPoseStamped::Request req_tool;
+ robot_mechanism_controllers::SetPoseStamped::Request req_tool;
req_tool.p = plug_pose_msg_;
- robot_srvs::SetPoseStamped::Response res_tool;
+ robot_mechanism_controllers::SetPoseStamped::Response res_tool;
if (!ros::service::call(servoing_controller_ + "/set_tool_frame", req_tool, res_tool))
{
ROS_ERROR("%s: Failed to set tool frame.", action_name_.c_str());
Modified: pkg/trunk/sandbox/experimental_controllers/CMakeLists.txt
===================================================================
--- pkg/trunk/sandbox/experimental_controllers/CMakeLists.txt 2009-08-04 22:36:32 UTC (rev 20720)
+++ pkg/trunk/sandbox/experimental_controllers/CMakeLists.txt 2009-08-04 22:37:08 UTC (rev 20721)
@@ -23,7 +23,6 @@
rospack_add_library(experimental_controllers
src/joint_trajectory_controller2.cpp
- src/plug_controller.cpp
src/trajectory_controller.cpp
src/pid_position_velocity_controller.cpp
)
Deleted: pkg/trunk/stacks/common_msgs/robot_srvs/srv/SetPoseStamped.srv
===================================================================
--- pkg/trunk/stacks/common_msgs/robot_srvs/srv/SetPoseStamped.srv 2009-08-04 22:36:32 UTC (rev 20720)
+++ pkg/trunk/stacks/common_msgs/robot_srvs/srv/SetPoseStamped.srv 2009-08-04 22:37:08 UTC (rev 20721)
@@ -1,2 +0,0 @@
-geometry_msgs/PoseStamped p
----
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <is...@us...> - 2009-08-05 03:42:10
|
Revision: 20749
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=20749&view=rev
Author: isucan
Date: 2009-08-05 03:42:01 +0000 (Wed, 05 Aug 2009)
Log Message:
-----------
new move_arm state machine; allow touching obstacles (with fingers)
Modified Paths:
--------------
pkg/trunk/demos/tabletop_manipulation/tabletop_scripts/src/simple.cpp
pkg/trunk/highlevel/move_arm/launch/move_larm.launch
pkg/trunk/highlevel/move_arm/launch/move_rarm.launch
pkg/trunk/highlevel/move_arm/src/arm_cmd_line.cpp
pkg/trunk/highlevel/move_arm/src/move_arm_action.cpp
pkg/trunk/motion_planning/ompl_planning/src/motion_planner.cpp
pkg/trunk/motion_planning/planning_environment/include/planning_environment/monitors/planning_monitor.h
pkg/trunk/motion_planning/planning_environment/src/monitors/planning_monitor.cpp
pkg/trunk/motion_planning/planning_environment/src/tools/view_state_validity.cpp
pkg/trunk/sandbox/3dnav_pr2/launch/planning/ompl_planning.launch
pkg/trunk/sandbox/3dnav_pr2/manifest.xml
Added Paths:
-----------
pkg/trunk/sandbox/fake_motion_planning/
pkg/trunk/sandbox/fake_motion_planning/CMakeLists.txt
pkg/trunk/sandbox/fake_motion_planning/Makefile
pkg/trunk/sandbox/fake_motion_planning/fake_motion_planner.cpp
pkg/trunk/sandbox/fake_motion_planning/mainpage.dox
pkg/trunk/sandbox/fake_motion_planning/manifest.xml
Removed Paths:
-------------
pkg/trunk/highlevel/move_arm/include/
Modified: pkg/trunk/demos/tabletop_manipulation/tabletop_scripts/src/simple.cpp
===================================================================
--- pkg/trunk/demos/tabletop_manipulation/tabletop_scripts/src/simple.cpp 2009-08-05 02:42:17 UTC (rev 20748)
+++ pkg/trunk/demos/tabletop_manipulation/tabletop_scripts/src/simple.cpp 2009-08-05 03:42:01 UTC (rev 20749)
@@ -202,7 +202,7 @@
else
{
ROS_ERROR("Gripper close failed");
- return false;
+ return true;
}
}
@@ -215,7 +215,7 @@
else
{
ROS_ERROR("Gripper open failed");
- return false;
+ return true;
}
}
@@ -231,7 +231,8 @@
pubObj.publish(o);
mapping_msgs::AttachedObject ao;
- ao.header = obj.object_pose.header;
+ ao.header.frame_id = obj.object_pose.header.frame_id;
+ ao.header.stamp = ros::Time::now();
ao.link_name = "r_wrist_roll_link";
ao.objects.push_back(obj.object);
ao.poses.push_back(obj.object_pose.pose);
@@ -286,6 +287,7 @@
if (ct.findObject(obj))
{
+ sleep(10);
if (ct.moveTo(obj))
{
ct.graspObject(obj);
Modified: pkg/trunk/highlevel/move_arm/launch/move_larm.launch
===================================================================
--- pkg/trunk/highlevel/move_arm/launch/move_larm.launch 2009-08-05 02:42:17 UTC (rev 20748)
+++ pkg/trunk/highlevel/move_arm/launch/move_larm.launch 2009-08-05 03:42:01 UTC (rev 20749)
@@ -13,11 +13,15 @@
<remap from="controller_query" to="/l_arm_joint_waypoint_controller/TrajectoryQuery" />
<remap from="controller_cancel" to="/l_arm_joint_waypoint_controller/TrajectoryCancel" />
- <remap from="get_motion_plan" to="plan_kinematic_path" />
-
- <param name="arm" type="string" value="left_arm" />
+ <remap from="get_motion_plan_lr" to="ompl_planning/plan_kinematic_path" />
+ <remap from="get_motion_plan_sr" to="fake_motion_planning/plan_kinematic_path" />
+
+ <param name="group" type="string" value="left_arm" />
<param name="show_collisions" type="bool" value="true" />
+ <param name="allowed_touch" type="string" value="l_gripper_l_finger_link l_gripper_r_finger_link l_gripper_l_finger_tip_link l_gripper_r_finger_tip_link" />
+ <param name="max_penetration_depth" type="double" value="0.01" />
+
<param name="refresh_interval_collision_map" type="double" value="20.0" />
<param name="refresh_interval_kinematic_state" type="double" value="1.0" />
<param name="bounding_planes" type="string" value="0 0 1 -0.01" />
Modified: pkg/trunk/highlevel/move_arm/launch/move_rarm.launch
===================================================================
--- pkg/trunk/highlevel/move_arm/launch/move_rarm.launch 2009-08-05 02:42:17 UTC (rev 20748)
+++ pkg/trunk/highlevel/move_arm/launch/move_rarm.launch 2009-08-05 03:42:01 UTC (rev 20749)
@@ -13,11 +13,15 @@
<remap from="controller_query" to="/r_arm_joint_waypoint_controller/TrajectoryQuery" />
<remap from="controller_cancel" to="/r_arm_joint_waypoint_controller/TrajectoryCancel" />
- <remap from="get_motion_plan" to="plan_kinematic_path" />
+ <remap from="get_motion_plan_lr" to="ompl_planning/plan_kinematic_path" />
+ <remap from="get_motion_plan_sr" to="fake_motion_planning/plan_kinematic_path" />
- <param name="arm" type="string" value="right_arm" />
+ <param name="group" type="string" value="right_arm" />
<param name="show_collisions" type="bool" value="true" />
+ <param name="allowed_touch" type="string" value="r_gripper_l_finger_link r_gripper_r_finger_link r_gripper_l_finger_tip_link r_gripper_r_finger_tip_link" />
+ <param name="max_penetration_depth" type="double" value="0.01" />
+
<param name="refresh_interval_collision_map" type="double" value="20.0" />
<param name="refresh_interval_kinematic_state" type="double" value="1.0" />
<param name="bounding_planes" type="string" value="0 0 1 -0.01" />
Modified: pkg/trunk/highlevel/move_arm/src/arm_cmd_line.cpp
===================================================================
--- pkg/trunk/highlevel/move_arm/src/arm_cmd_line.cpp 2009-08-05 02:42:17 UTC (rev 20748)
+++ pkg/trunk/highlevel/move_arm/src/arm_cmd_line.cpp 2009-08-05 03:42:01 UTC (rev 20749)
@@ -163,12 +163,12 @@
goal.goal_constraints.pose_constraint[0].pose.pose.orientation.z = pz[5];
goal.goal_constraints.pose_constraint[0].pose.pose.orientation.w = pz[6];
- goal.goal_constraints.pose_constraint[0].position_tolerance_above.x = 0.001;
- goal.goal_constraints.pose_constraint[0].position_tolerance_above.y = 0.001;
- goal.goal_constraints.pose_constraint[0].position_tolerance_above.z = 0.002;
- goal.goal_constraints.pose_constraint[0].position_tolerance_below.x = 0.001;
- goal.goal_constraints.pose_constraint[0].position_tolerance_below.y = 0.001;
- goal.goal_constraints.pose_constraint[0].position_tolerance_below.z = 0.002;
+ goal.goal_constraints.pose_constraint[0].position_tolerance_above.x = 0.005;
+ goal.goal_constraints.pose_constraint[0].position_tolerance_above.y = 0.005;
+ goal.goal_constraints.pose_constraint[0].position_tolerance_above.z = 0.01;
+ goal.goal_constraints.pose_constraint[0].position_tolerance_below.x = 0.005;
+ goal.goal_constraints.pose_constraint[0].position_tolerance_below.y = 0.01;
+ goal.goal_constraints.pose_constraint[0].position_tolerance_below.z = 0.005;
goal.goal_constraints.pose_constraint[0].orientation_tolerance_above.x = 0.005;
goal.goal_constraints.pose_constraint[0].orientation_tolerance_above.y = 0.005;
Modified: pkg/trunk/highlevel/move_arm/src/move_arm_action.cpp
===================================================================
--- pkg/trunk/highlevel/move_arm/src/move_arm_action.cpp 2009-08-05 02:42:17 UTC (rev 20748)
+++ pkg/trunk/highlevel/move_arm/src/move_arm_action.cpp 2009-08-05 03:42:01 UTC (rev 20749)
@@ -35,145 +35,596 @@
*
* Authors: Sachin Chitta, Ioan Sucan
*********************************************************************/
-#include <move_arm/move_arm.h>
+#include <ros/ros.h>
+
+#include <robot_actions/action.h>
+#include <robot_actions/action_runner.h>
+#include <pr2_robot_actions/MoveArmState.h>
+#include <pr2_robot_actions/MoveArmGoal.h>
+
+#include <manipulation_msgs/JointTraj.h>
+#include <manipulation_srvs/IKService.h>
+
+
#include <pr2_mechanism_controllers/TrajectoryStart.h>
#include <pr2_mechanism_controllers/TrajectoryQuery.h>
#include <pr2_mechanism_controllers/TrajectoryCancel.h>
-#include <manipulation_srvs/IKService.h>
+#include <motion_planning_msgs/GetMotionPlan.h>
#include <motion_planning_msgs/ConvertToJointConstraint.h>
#include <visualization_msgs/Marker.h>
+
+#include <planning_environment/monitors/planning_monitor.h>
+#include <algorithm>
#include <cstdlib>
using namespace robot_actions;
-namespace move_arm
+/// the string used internally to access control starting service; this should be remaped in the launch file
+static const std::string CONTROL_START_NAME = "controller_start";
+
+/// the string used internally to access control querying service; this should be remaped in the launch file
+static const std::string CONTROL_QUERY_NAME = "controller_query";
+
+/// the string used internally to access control canceling service; this should be remaped in the launch file
+static const std::string CONTROL_CANCEL_NAME = "controller_cancel";
+
+/// the string used internally to access the long range motion planning service; this should be remaped in the launch file
+static const std::string LR_MOTION_PLAN_NAME = "get_motion_plan_lr";
+
+/// the string used internally to access the short range motion planning service; this should be remaped in the launch file
+static const std::string SR_MOTION_PLAN_NAME = "get_motion_plan_sr";
+
+/// the string used internally to access valid state searching service; this should be remaped in the launch file
+static const std::string SEARCH_VALID_STATE_NAME = "get_valid_state";
+
+/// the string used internally to access inverse kinematics service; this should be remaped in the launch file
+static const std::string ARM_IK_NAME = "arm_ik";
+
+
+/** \brief Configuration of actions that need to actuate parts of the robot */
+class MoveBodyCore
{
- // these are the strings used internally to access services
- // they should be remaped in the launch file
- static const std::string CONTROL_START_NAME = "controller_start";
- static const std::string CONTROL_QUERY_NAME = "controller_query";
- static const std::string CONTROL_CANCEL_NAME = "controller_cancel";
- static const std::string MOTION_PLAN_NAME = "get_motion_plan";
- static const std::string SEARCH_VALID_STATE_NAME = "get_valid_state";
- static const std::string ARM_IK_NAME = "arm_ik";
+ friend class MoveArm;
+
+public:
+
+ MoveBodyCore(void)
+ {
+ collisionModels_ = NULL;
+ planningMonitor_ = NULL;
+ }
+
+ virtual ~MoveBodyCore(void)
+ {
+ if (planningMonitor_)
+ delete planningMonitor_;
+ if (collisionModels_)
+ delete collisionModels_;
+ }
+
+
+ bool configure(void)
+ {
+ nodeHandle_.param<std::string>("~group", group_, std::string());
+
+ if (group_.empty())
+ {
+ ROS_ERROR("No '~group' parameter specified. Without the name of the group of joints to plan for, action cannot start");
+ return false;
+ }
- MoveArm::MoveArm(const::std::string &arm_name) : Action<pr2_robot_actions::MoveArmGoal, int32_t>("move_" + arm_name)
- {
- valid_ = true;
- arm_ = arm_name;
-
- node_handle_.param<bool>("~perform_ik", perform_ik_, true);
- node_handle_.param<bool>("~show_collisions", show_collisions_, false);
- node_handle_.param<bool>("~unsafe_paths", unsafe_paths_, false);
-
// monitor robot
collisionModels_ = new planning_environment::CollisionModels("robot_description");
planningMonitor_ = new planning_environment::PlanningMonitor(collisionModels_, &tf_);
-
- if (collisionModels_->getKinematicModel()->getGroupID(arm_) < 0)
+
+ if (!collisionModels_->loadedModels())
+ return false;
+
+ nodeHandle_.param<bool>("~perform_ik", perform_ik_, true);
+
+ if (collisionModels_->getKinematicModel()->getGroupID(group_) < 0)
{
- valid_ = false;
- ROS_ERROR("Arm '%s' is not known", arm_.c_str());
+ ROS_ERROR("Group '%s' is not known", group_.c_str());
+ return false;
}
else
- ROS_INFO("Starting move_arm for '%s' (IK is %senabled)", arm_.c_str(), perform_ik_ ? "" : "not ");
+ ROS_INFO("Configuring action core for '%s' (IK is %senabled)", group_.c_str(), perform_ik_ ? "" : "not ");
- if (valid_)
- valid_ = collisionModels_->loadedModels();
+ planningMonitor_->waitForState();
+ planningMonitor_->waitForMap();
- if (valid_)
+ if (!getControlJointNames(groupJointNames_))
+ return false;
+
+ nodeHandle_.param<bool>("~show_collisions", show_collisions_, false);
+ nodeHandle_.param<bool>("~unsafe_paths", unsafe_paths_, false);
+ if (show_collisions_)
+ ROS_INFO("Found collisions will be displayed as visualization markers");
+
+ if (unsafe_paths_)
+ ROS_WARN("Paths will NOT be monitored for collision once they have been sent to the controller");
+
+ std::string touch;
+ nodeHandle_.param<std::string>("~allowed_touch", touch, std::string());
+ std::stringstream ss(touch);
+ while (ss.good() & !ss.eof())
{
- if (show_collisions_)
+ std::string link; ss >> link;
+ allowedTouch_[link] = true;
+ }
+
+ if (!touch.empty())
+ ROS_INFO("Touching is allowed for links: %s", touch.c_str());
+ else
+ ROS_INFO("No link is allowed to touch objects");
+
+ nodeHandle_.param<double>("~max_penetration_depth", maxPenetrationDepth_, 0.0);
+ if (!touch.empty())
+ ROS_INFO("Maximum penetration depth is %f", maxPenetrationDepth_);
+
+ return true;
+ }
+
+protected:
+
+ bool getControlJointNames(std::vector<std::string> &joint_names)
+ {
+ ros::ServiceClient client_query = nodeHandle_.serviceClient<pr2_mechanism_controllers::TrajectoryQuery>(CONTROL_QUERY_NAME);
+ pr2_mechanism_controllers::TrajectoryQuery::Request req_query;
+ pr2_mechanism_controllers::TrajectoryQuery::Response res_query;
+ req_query.trajectoryid = pr2_mechanism_controllers::TrajectoryQuery::Request::Query_Joint_Names;
+
+ bool result = client_query.call(req_query, res_query);
+
+ if (!result)
+ {
+ ROS_INFO("Querying controller for joint names ...");
+ ros::Duration(5.0).sleep();
+ result = client_query.call(req_query, res_query);
+ if (result)
+ ROS_INFO("Joint names received");
+ }
+
+ if (!result)
+ {
+ ROS_ERROR("Unable to retrieve controller joint names from control query service");
+ return false;
+ }
+
+ joint_names = res_query.jointnames;
+
+ // make sure we have the right joint names
+ for(unsigned int i = 0; i < joint_names.size() ; ++i)
+ {
+ if (planning_models::KinematicModel::Joint *j = planningMonitor_->getKinematicModel()->getJoint(joint_names[i]))
{
- ROS_INFO("Found collisions will be displayed as visualization markers");
- visMarkerPublisher_ = node_handle_.advertise<visualization_msgs::Marker>("visualization_marker", 128);
- planningMonitor_->setOnCollisionContactCallback(boost::bind(&MoveArm::contactFound, this, _1));
+ ROS_DEBUG("Using joing '%s' with %u parameters", j->name.c_str(), j->usedParams);
+ if (planningMonitor_->getKinematicModel()->getJointIndexInGroup(j->name, group_) < 0)
+ return false;
}
- planningMonitor_->getEnvironmentModel()->setVerbose(true);
- planningMonitor_->waitForState();
- planningMonitor_->waitForMap();
- valid_ = getControlJointNames(arm_joint_names_);
+ else
+ {
+ ROS_ERROR("Joint '%s' is not known", joint_names[i].c_str());
+ return false;
+ }
}
- if (!valid_)
- ROS_ERROR("Move arm action is invalid");
+ std::vector<std::string> groupNames;
+ planningMonitor_->getKinematicModel()->getJointsInGroup(groupNames, group_);
+ if (groupNames.size() != joint_names.size())
+ {
+ ROS_ERROR("The group '%s' does not have the same number of joints as the controller can handle", group_.c_str());
+ return false;
+ }
+ return true;
+ }
+
+ ros::NodeHandle nodeHandle_;
+ tf::TransformListener tf_;
+ planning_environment::CollisionModels *collisionModels_;
+ planning_environment::PlanningMonitor *planningMonitor_;
+
+ std::string group_;
+ std::vector<std::string> groupJointNames_;
+ bool perform_ik_;
+ bool unsafe_paths_;
+ bool show_collisions_;
+
+ /// the set of links that are allowed to touch objects
+ std::map<std::string, bool> allowedTouch_;
+
+ /// the maximum penetration allowed when touching objects
+ double maxPenetrationDepth_;
+
+};
+
+
+class MoveArm : public robot_actions::Action<pr2_robot_actions::MoveArmGoal, int32_t>
+{
+public:
+
+ MoveArm(MoveBodyCore &core) : Action<pr2_robot_actions::MoveArmGoal, int32_t>("move_" + core.group_), core_(core)
+ {
+ if (core_.show_collisions_)
+ visMarkerPublisher_ = core_.nodeHandle_.advertise<visualization_msgs::Marker>("visualization_marker", 128);
+
// advertise the topic for displaying kinematic plans
- displayPathPublisher_ = node_handle_.advertise<motion_planning_msgs::KinematicPath>("display_kinematic_path", 10);
+ displayPathPublisher_ = core_.nodeHandle_.advertise<motion_planning_msgs::KinematicPath>("display_kinematic_path", 10);
+
+ validatingPath_ = false;
+ planningMonitor_ = core_.planningMonitor_;
+ planningMonitor_->setOnCollisionContactCallback(boost::bind(&MoveArm::contactFound, this, _1), 0);
+ planningMonitor_->getEnvironmentModel()->setVerbose(false);
}
- MoveArm::~MoveArm()
+ virtual ~MoveArm()
{
- delete planningMonitor_;
- delete collisionModels_;
}
+
+private:
+
+ /** \brief Evaluate the cost of a state, in terms of collisions */
+ double computeStateCollisionCost(const planning_models::StateParams *sp)
+ {
+ // clear collision data where callback adds to
+ collisionCost_ = 0.0;
+ collisionLinks_.clear();
- robot_actions::ResultStatus MoveArm::execute(const pr2_robot_actions::MoveArmGoal& goal, int32_t& feedback)
- {
- if (!valid_)
+ // check for collision, getting all contacts
+ planningMonitor_->isStateValid(sp, planning_environment::PlanningMonitor::COLLISION_TEST);
+
+ return evaluateLastCollisionCost();
+ }
+
+ /** \brief Evaluate the cost of the last set of collision checks */
+ double evaluateLastCollisionCost(void)
+ {
+ // check we are only touching with allowed links
+ for (std::map<std::string, int>::iterator it = collisionLinks_.begin() ; it != collisionLinks_.end() ; ++it)
+ if (core_.allowedTouch_.find(it->first) == core_.allowedTouch_.end())
+ return INFINITY;
+ return collisionCost_;
+ }
+
+ /** \brief If we have a complex goal for which we have not yet found a valid goal state, we use this function*/
+ robot_actions::ResultStatus solveGoalComplex(std::vector< boost::shared_ptr<planning_models::StateParams> > &states,
+ motion_planning_msgs::GetMotionPlan::Request &req, int32_t& feedback)
+ {
+ ROS_DEBUG("Acting on goal with unknown valid goal state ...");
+
+
+ // we make a request to a service that attempts to find a valid state close to the goal
+ motion_planning_msgs::ConvertToJointConstraint::Request c_req;
+ c_req.params = req.params;
+ c_req.start_state = req.start_state;
+ c_req.constraints = req.goal_constraints;
+ c_req.names = core_.groupJointNames_;
+ c_req.states.resize(states.size());
+ c_req.allowed_time = 1.0;
+
+ // if we have hints about where the goal might be, we set them here
+ for (unsigned int i = 0 ; i < states.size() ; ++i)
+ states[i]->copyParamsJoints(c_req.states[i].vals, core_.groupJointNames_);
+
+ motion_planning_msgs::ConvertToJointConstraint::Response c_res;
+ ros::ServiceClient s_client = core_.nodeHandle_.serviceClient<motion_planning_msgs::ConvertToJointConstraint>(SEARCH_VALID_STATE_NAME);
+ if (s_client.call(c_req, c_res))
{
- ROS_ERROR("Move arm action has not been initialized properly");
- return robot_actions::ABORTED;
+ // if we found a valid state
+ if (!c_res.joint_constraint.empty())
+ {
+
+ // construct a state representation from our goal joint
+ boost::shared_ptr<planning_models::StateParams> sp(new planning_models::StateParams(*planningMonitor_->getRobotState()));
+
+ for (unsigned int i = 0 ; i < c_res.joint_constraint.size() ; ++i)
+ {
+ const motion_planning_msgs::JointConstraint &kj = c_res.joint_constraint[i];
+ sp->setParamsJoint(kj.value, kj.joint_name);
+ }
+ sp->enforceBounds();
+
+ // if the state is in fact in the goal region, simply run the LR planner
+ if (planningMonitor_->isStateValidAtGoal(sp.get()))
+ {
+ ROS_DEBUG("Found valid goal state ...");
+
+ req.goal_constraints.joint_constraint = c_res.joint_constraint;
+ req.goal_constraints.pose_constraint.clear();
+
+ // update the goal constraints for the planning monitor as well
+ planningMonitor_->setGoalConstraints(req.goal_constraints);
+
+ return runLRplanner(req, feedback);
+ }
+ else
+ {
+ // if the state is valid but not in the goal region,
+ // we plan in two steps: first to this intermediate state
+ // that we hope is close to the goal and second to the final goal position
+ // using the SR planner
+ ROS_DEBUG("Found intermediate state ...");
+
+ motion_planning_msgs::KinematicConstraints kc = req.goal_constraints;
+
+ req.goal_constraints.joint_constraint = c_res.joint_constraint;
+ req.goal_constraints.pose_constraint.clear();
+
+ // update the goal constraints for the planning monitor as well
+ planningMonitor_->setGoalConstraints(req.goal_constraints);
+
+ robot_actions::ResultStatus result = runLRplanner(req, feedback);
+
+ req.goal_constraints = kc;
+ planningMonitor_->setGoalConstraints(req.goal_constraints);
+
+ // if reaching the intermediate state was succesful
+ if (result == robot_actions::SUCCESS)
+ {
+ // run the short range planner to the original goal
+ robot_actions::ResultStatus temp = runSRplanner(req, feedback);
+
+ // if the planner aborted and we have an idea about an invalid state that
+ // may be in the goal region, we make one last try using the short range planner
+ if (temp == robot_actions::ABORTED && !states.empty())
+ {
+ // set the goal to be a state
+ updateRequest(req, states[0].get());
+ temp = runSRplanner(req, feedback);
+
+ // restore the input received from the user
+ req.goal_constraints = kc;
+ planningMonitor_->setGoalConstraints(req.goal_constraints);
+
+ return temp;
+ }
+ else
+ return temp;
+ }
+ else
+ return result;
+ }
+ }
+ else
+ return runLRplanner(req, feedback);
}
+ else
+ {
+ ROS_ERROR("Service for searching for valid states failed");
+ return runLRplanner(req, feedback);
+ }
- motion_planning_msgs::GetMotionPlan::Request req;
- motion_planning_msgs::GetMotionPlan::Response res;
+ }
+
+ /** \brief Extract the state specified by the goal and run a planner towards it, if it is valid */
+ robot_actions::ResultStatus solveGoalJoints(motion_planning_msgs::GetMotionPlan::Request &req, int32_t& feedback)
+ {
+ ROS_DEBUG("Acting on goal to set of joints ...");
+
+ // construct a state representation from our goal joint
+ boost::shared_ptr<planning_models::StateParams> sp(new planning_models::StateParams(*planningMonitor_->getRobotState()));
+ for (unsigned int i = 0 ; i < req.goal_constraints.joint_constraint.size() ; ++i)
+ {
+ const motion_planning_msgs::JointConstraint &kj = req.goal_constraints.joint_constraint[i];
+ sp->setParamsJoint(kj.value, kj.joint_name);
+ }
+ sp->enforceBounds();
- req.params.model_id = arm_; // the model to plan for (should be defined in planning.yaml)
- req.params.distance_metric = "L2Square"; // the metric to be used in the robot's state space
-
- // forward the goal & path constraints
- req.goal_constraints = goal.goal_constraints;
- req.path_constraints = goal.path_constraints;
+ // try to skip straight to planning
+ if (planningMonitor_->isStateValidAtGoal(sp.get()))
+ return runLRplanner(req, feedback);
+ else
+ {
+ // if we can't, go to the more generic joint solver
+ std::vector< boost::shared_ptr<planning_models::StateParams> > states;
+ states.push_back(sp);
+ return solveGoalJoints(states, req, feedback);
+ }
+ }
+
+ // construct a list of states with cost
+ struct myState
+ {
+ planning_models::StateParams *state;
+ double cost;
+ unsigned int index;
+ };
+
+ struct myStateComp
+ {
+ bool operator()(const myState& a, const myState& b) const
+ {
+ return a.cost < b.cost;
+ }
+ };
+
+ void updateRequest(motion_planning_msgs::GetMotionPlan::Request &req, const planning_models::StateParams *sp)
+ {
+ // update request
+ for (unsigned int i = 0 ; i < core_.groupJointNames_.size() ; ++i)
+ {
+ motion_planning_msgs::JointConstraint jc;
+ jc.joint_name = core_.groupJointNames_[i];
+ jc.header.frame_id = planningMonitor_->getFrameId();
+ jc.header.stamp = planningMonitor_->lastJointStateUpdate();
+ sp->copyParamsJoint(jc.value, core_.groupJointNames_[i]);
+ jc.tolerance_above.resize(jc.value.size(), 0.0);
+ jc.tolerance_below.resize(jc.value.size(), 0.0);
+ req.goal_constraints.joint_constraint.push_back(jc);
+ }
+ req.goal_constraints.pose_constraint.clear();
- // compute the path once
- req.times = 1;
+ // update the goal constraints for the planning monitor as well
+ planningMonitor_->setGoalConstraints(req.goal_constraints);
+ }
+
+ /** \brief Find a plan to given request, given a set of hint states in the goal region */
+ robot_actions::ResultStatus solveGoalJoints(std::vector< boost::shared_ptr<planning_models::StateParams> > &states,
+ motion_planning_msgs::GetMotionPlan::Request &req, int32_t& feedback)
+ {
+ ROS_DEBUG("Acting on goal to set of joints pointing to potentially invalid state ...");
+
+ // just in case we received no states (this should not happen)
+ if (states.empty())
+ return solveGoalComplex(states, req, feedback);
+
+ std::vector<myState> cstates(states.size());
+
+ for (unsigned int i = 0 ; i < states.size() ; ++i)
+ {
+ cstates[i].state = states[i].get();
+ cstates[i].cost = computeStateCollisionCost(states[i].get());
+ cstates[i].index = i;
+ }
+
+ // find the state with minimal cost
+ std::sort(cstates.begin(), cstates.end(), myStateComp());
+
+ for (unsigned int i = 0 ; i < cstates.size() ; ++i)
+ ROS_DEBUG("Cost of hint state %d is %f", i, cstates[i].cost);
- // do not spend more than this amount of time
- req.allowed_time = 1.0;
+ if (planningMonitor_->isStateValidAtGoal(cstates[0].state))
+ {
+ updateRequest(req, cstates[0].state);
+ return runLRplanner(req, feedback);
+ }
+ else
+ {
+ // order the states by cost before passing them forward
+ std::vector< boost::shared_ptr<planning_models::StateParams> > backup = states;
+ for (unsigned int i = 0 ; i < cstates.size() ; ++i)
+ states[i] = backup[cstates[i].index];
+ return solveGoalComplex(states, req, feedback);
+ }
+ }
+
+ double uniformDouble(double lower_bound, double upper_bound)
+ {
+ return (upper_bound - lower_bound) * drand48() + lower_bound;
+ }
+
+ /** \brief We generate IK solutions in the goal region. We stop
+ generating possible solutions when we find a valid one or we
+ reach a maximal number of steps. If we have candidate
+ solutions, we forward this to the joint-goal solver. If not,
+ the complex goal solver is to be used. */
+ robot_actions::ResultStatus solveGoalPose(motion_planning_msgs::GetMotionPlan::Request &req, int32_t& feedback)
+ {
+ ROS_DEBUG("Acting on goal to pose ...");
- // tell the planning monitor about the constraints we will be following
- planningMonitor_->setPathConstraints(req.path_constraints);
- planningMonitor_->setGoalConstraints(req.goal_constraints);
+ // we do IK to find corresponding states
+ ros::ServiceClient ik_client = core_.nodeHandle_.serviceClient<manipulation_srvs::IKService>(ARM_IK_NAME, true);
+ std::vector< boost::shared_ptr<planning_models::StateParams> > states;
- // fill the staring state
- fillStartState(req.start_state);
+ // find an IK solution
+ for (int step = 0 ; step < 10 ; ++step)
+ {
+ std::vector<double> solution;
+
+ geometry_msgs::PoseStamped tpose = req.goal_constraints.pose_constraint[0].pose;
+
+ if (step > 0)
+ {
+ tpose.pose.position.x = uniformDouble(tpose.pose.position.x - req.goal_constraints.pose_constraint[0].position_tolerance_below.x,
+ tpose.pose.position.x + req.goal_constraints.pose_constraint[0].position_tolerance_above.x);
+ tpose.pose.position.y = uniformDouble(tpose.pose.position.y - req.goal_constraints.pose_constraint[0].position_tolerance_below.y,
+ tpose.pose.position.y + req.goal_constraints.pose_constraint[0].position_tolerance_above.y);
+ tpose.pose.position.z = uniformDouble(tpose.pose.position.z - req.goal_constraints.pose_constraint[0].position_tolerance_below.z,
+ tpose.pose.position.z + req.goal_constraints.pose_constraint[0].position_tolerance_above.z);
+ }
+
+ if (computeIK(ik_client, tpose, solution))
+ {
+ // check if it is a valid state
+ boost::shared_ptr<planning_models::StateParams> spTest(new planning_models::StateParams(*planningMonitor_->getRobotState()));
+ spTest->setParamsJoints(solution, core_.groupJointNames_);
+ spTest->enforceBounds();
+
+ states.push_back(spTest);
+ if (planningMonitor_->isStateValidAtGoal(spTest.get()))
+ break;
+ }
+ else
+ break;
+ }
+
+ if (states.empty())
+ return solveGoalComplex(states, req, feedback);
+ else
+ return solveGoalJoints(states, req, feedback);
+ }
+
+ /** \brief Depending on the type of constraint, decide whether or not to use IK, decide which planners to use */
+ robot_actions::ResultStatus solveGoal(motion_planning_msgs::GetMotionPlan::Request &req, int32_t& feedback)
+ {
+ ROS_DEBUG("Acting on goal...");
+
+ // change pose constraints to joint constraints, if possible...
[truncated message content] |
|
From: <ge...@us...> - 2009-08-05 06:38:26
|
Revision: 20761
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=20761&view=rev
Author: gerkey
Date: 2009-08-05 06:38:16 +0000 (Wed, 05 Aug 2009)
Log Message:
-----------
Fixed stack dependencies and added Makefiles and CMakeLists.txts.
Modified Paths:
--------------
pkg/trunk/3rdparty/stack.xml
pkg/trunk/applications/2dmapping_pr2_app/stack.xml
pkg/trunk/applications/2dnav_pr2_app/stack.xml
pkg/trunk/audio/stack.xml
pkg/trunk/calibration/stack.xml
pkg/trunk/controllers/stack.xml
pkg/trunk/drivers/stack.xml
pkg/trunk/highlevel/stack.xml
pkg/trunk/openrave_planning/stack.xml
pkg/trunk/pr2/stack.xml
pkg/trunk/stacks/common/stack.xml
pkg/trunk/stacks/common_msgs/stack.xml
pkg/trunk/stacks/driver_common/stack.xml
pkg/trunk/stacks/estimation/stack.xml
pkg/trunk/stacks/ethercat_drivers/stack.xml
pkg/trunk/stacks/geometry/stack.xml
pkg/trunk/stacks/hardware_test/stack.xml
pkg/trunk/stacks/mechanism/stack.xml
pkg/trunk/stacks/motion_planning/stack.xml
pkg/trunk/stacks/navigation/stack.xml
pkg/trunk/stacks/opencv/stack.xml
pkg/trunk/stacks/semantic_mapping/stack.xml
pkg/trunk/stacks/simulators/stack.xml
pkg/trunk/stacks/sound_drivers/stack.xml
pkg/trunk/stacks/visualization/stack.xml
pkg/trunk/stacks/visualization_core/stack.xml
pkg/trunk/stacks/world_models/stack.xml
pkg/trunk/util/stack.xml
pkg/trunk/vision/stack.xml
Added Paths:
-----------
pkg/trunk/3rdparty/CMakeLists.txt
pkg/trunk/3rdparty/Makefile
pkg/trunk/applications/2dmapping_pr2_app/CMakeLists.txt
pkg/trunk/applications/2dmapping_pr2_app/Makefile
pkg/trunk/applications/2dnav_pr2_app/CMakeLists.txt
pkg/trunk/applications/2dnav_pr2_app/Makefile
pkg/trunk/audio/CMakeLists.txt
pkg/trunk/audio/Makefile
pkg/trunk/calibration/CMakeLists.txt
pkg/trunk/calibration/Makefile
pkg/trunk/controllers/CMakeLists.txt
pkg/trunk/controllers/Makefile
pkg/trunk/drivers/CMakeLists.txt
pkg/trunk/drivers/Makefile
pkg/trunk/highlevel/CMakeLists.txt
pkg/trunk/highlevel/Makefile
pkg/trunk/openrave_planning/CMakeLists.txt
pkg/trunk/openrave_planning/Makefile
pkg/trunk/pr2/CMakeLists.txt
pkg/trunk/pr2/Makefile
pkg/trunk/stacks/common/CMakeLists.txt
pkg/trunk/stacks/common/Makefile
pkg/trunk/stacks/driver_common/CMakeLists.txt
pkg/trunk/stacks/driver_common/Makefile
pkg/trunk/stacks/estimation/CMakeLists.txt
pkg/trunk/stacks/estimation/Makefile
pkg/trunk/stacks/ethercat_drivers/CMakeLists.txt
pkg/trunk/stacks/ethercat_drivers/Makefile
pkg/trunk/stacks/geometry/CMakeLists.txt
pkg/trunk/stacks/geometry/Makefile
pkg/trunk/stacks/geometry/kdl/Makefile.orig
pkg/trunk/stacks/hardware_test/CMakeLists.txt
pkg/trunk/stacks/hardware_test/Makefile
pkg/trunk/stacks/mechanism/CMakeLists.txt
pkg/trunk/stacks/mechanism/Makefile
pkg/trunk/stacks/motion_planning/CMakeLists.txt
pkg/trunk/stacks/motion_planning/Makefile
pkg/trunk/stacks/navigation/CMakeLists.txt
pkg/trunk/stacks/navigation/Makefile
pkg/trunk/stacks/opencv/CMakeLists.txt
pkg/trunk/stacks/opencv/Makefile
pkg/trunk/stacks/semantic_mapping/CMakeLists.txt
pkg/trunk/stacks/semantic_mapping/Makefile
pkg/trunk/stacks/simulators/CMakeLists.txt
pkg/trunk/stacks/simulators/Makefile
pkg/trunk/stacks/sound_drivers/CMakeLists.txt
pkg/trunk/stacks/sound_drivers/Makefile
pkg/trunk/stacks/visualization/CMakeLists.txt
pkg/trunk/stacks/visualization/Makefile
pkg/trunk/stacks/visualization_core/CMakeLists.txt
pkg/trunk/stacks/visualization_core/Makefile
pkg/trunk/stacks/world_models/CMakeLists.txt
pkg/trunk/stacks/world_models/Makefile
pkg/trunk/util/CMakeLists.txt
pkg/trunk/util/Makefile
pkg/trunk/vision/CMakeLists.txt
pkg/trunk/vision/Makefile
Added: pkg/trunk/3rdparty/CMakeLists.txt
===================================================================
--- pkg/trunk/3rdparty/CMakeLists.txt (rev 0)
+++ pkg/trunk/3rdparty/CMakeLists.txt 2009-08-05 06:38:16 UTC (rev 20761)
@@ -0,0 +1,21 @@
+cmake_minimum_required(VERSION 2.4.6)
+include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
+
+set(ROSPACK_MAKEDIST true)
+
+# Append to CPACK_SOURCE_IGNORE_FILES a semicolon-separated list of
+# directories (or patterns, but directories should suffice) that should
+# be excluded from the distro. This is not the place to put things that
+# should be ignored everywhere, like "build" directories; that happens in
+# rosbuild/rosbuild.cmake. Here should be listed packages that aren't
+# ready for inclusion in a distro.
+#
+# This list is combined with the list in rosbuild/rosbuild.cmake. Note
+# that CMake 2.6 may be required to ensure that the two lists are combined
+# properly. CMake 2.4 seems to have unpredictable scoping rules for such
+# variables.
+#list(APPEND CPACK_SOURCE_IGNORE_FILES /core/experimental)
+
+rosbuild(3rdparty 0.1.0)
+# After next ROS release, change to new macro
+#rosbuild_make_distribution(0.1.0)
Added: pkg/trunk/3rdparty/Makefile
===================================================================
--- pkg/trunk/3rdparty/Makefile (rev 0)
+++ pkg/trunk/3rdparty/Makefile 2009-08-05 06:38:16 UTC (rev 20761)
@@ -0,0 +1 @@
+include $(shell rospack find mk)/cmake_stack.mk
Modified: pkg/trunk/3rdparty/stack.xml
===================================================================
--- pkg/trunk/3rdparty/stack.xml 2009-08-05 06:33:38 UTC (rev 20760)
+++ pkg/trunk/3rdparty/stack.xml 2009-08-05 06:38:16 UTC (rev 20761)
@@ -1,13 +1,12 @@
<stack name="3rdparty" version="0.1">
<description brief="3rdparty packages from ros-pkg">
- These are common dependencies for ros-pkg
</description>
- <version>0.1</version>
<author>Tully Foote tf...@wi...</author>
<license>BSD</license>
<review status="unreviewed" notes=""/>
<url>http://pr.willowgarage.com/wiki/3rdparty</url>
- <depend stack="ros" version="0.5"/>
-</stack>
+ <depend stack="ros"/> <!-- roscpp, std_msgs, rospy, std_msgs -->
+
+</stack>
Added: pkg/trunk/applications/2dmapping_pr2_app/CMakeLists.txt
===================================================================
--- pkg/trunk/applications/2dmapping_pr2_app/CMakeLists.txt (rev 0)
+++ pkg/trunk/applications/2dmapping_pr2_app/CMakeLists.txt 2009-08-05 06:38:16 UTC (rev 20761)
@@ -0,0 +1,21 @@
+cmake_minimum_required(VERSION 2.4.6)
+include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
+
+set(ROSPACK_MAKEDIST true)
+
+# Append to CPACK_SOURCE_IGNORE_FILES a semicolon-separated list of
+# directories (or patterns, but directories should suffice) that should
+# be excluded from the distro. This is not the place to put things that
+# should be ignored everywhere, like "build" directories; that happens in
+# rosbuild/rosbuild.cmake. Here should be listed packages that aren't
+# ready for inclusion in a distro.
+#
+# This list is combined with the list in rosbuild/rosbuild.cmake. Note
+# that CMake 2.6 may be required to ensure that the two lists are combined
+# properly. CMake 2.4 seems to have unpredictable scoping rules for such
+# variables.
+#list(APPEND CPACK_SOURCE_IGNORE_FILES /core/experimental)
+
+rosbuild(2dmapping_pr2_app 0.1.0)
+# After next ROS release, change to new macro
+#rosbuild_make_distribution(0.1.0)
Added: pkg/trunk/applications/2dmapping_pr2_app/Makefile
===================================================================
--- pkg/trunk/applications/2dmapping_pr2_app/Makefile (rev 0)
+++ pkg/trunk/applications/2dmapping_pr2_app/Makefile 2009-08-05 06:38:16 UTC (rev 20761)
@@ -0,0 +1 @@
+include $(shell rospack find mk)/cmake_stack.mk
Modified: pkg/trunk/applications/2dmapping_pr2_app/stack.xml
===================================================================
--- pkg/trunk/applications/2dmapping_pr2_app/stack.xml 2009-08-05 06:33:38 UTC (rev 20760)
+++ pkg/trunk/applications/2dmapping_pr2_app/stack.xml 2009-08-05 06:38:16 UTC (rev 20761)
@@ -6,7 +6,7 @@
<review status="unreviewed" notes=""/>
<url>http://pr.willowgarage.com/wiki/2dmapping_pr2_app</url>
- <depend stack="ros" version="0.7.0"/>
- <!-- TODO: put proper stack dependencies in here -->
+
+ <depend stack="ros"/> <!-- rosrecord -->
+
</stack>
-
Added: pkg/trunk/applications/2dnav_pr2_app/CMakeLists.txt
===================================================================
--- pkg/trunk/applications/2dnav_pr2_app/CMakeLists.txt (rev 0)
+++ pkg/trunk/applications/2dnav_pr2_app/CMakeLists.txt 2009-08-05 06:38:16 UTC (rev 20761)
@@ -0,0 +1,21 @@
+cmake_minimum_required(VERSION 2.4.6)
+include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
+
+set(ROSPACK_MAKEDIST true)
+
+# Append to CPACK_SOURCE_IGNORE_FILES a semicolon-separated list of
+# directories (or patterns, but directories should suffice) that should
+# be excluded from the distro. This is not the place to put things that
+# should be ignored everywhere, like "build" directories; that happens in
+# rosbuild/rosbuild.cmake. Here should be listed packages that aren't
+# ready for inclusion in a distro.
+#
+# This list is combined with the list in rosbuild/rosbuild.cmake. Note
+# that CMake 2.6 may be required to ensure that the two lists are combined
+# properly. CMake 2.4 seems to have unpredictable scoping rules for such
+# variables.
+#list(APPEND CPACK_SOURCE_IGNORE_FILES /core/experimental)
+
+rosbuild(2dnav_pr2_app 0.1.0)
+# After next ROS release, change to new macro
+#rosbuild_make_distribution(0.1.0)
Added: pkg/trunk/applications/2dnav_pr2_app/Makefile
===================================================================
--- pkg/trunk/applications/2dnav_pr2_app/Makefile (rev 0)
+++ pkg/trunk/applications/2dnav_pr2_app/Makefile 2009-08-05 06:38:16 UTC (rev 20761)
@@ -0,0 +1 @@
+include $(shell rospack find mk)/cmake_stack.mk
Modified: pkg/trunk/applications/2dnav_pr2_app/stack.xml
===================================================================
--- pkg/trunk/applications/2dnav_pr2_app/stack.xml 2009-08-05 06:33:38 UTC (rev 20760)
+++ pkg/trunk/applications/2dnav_pr2_app/stack.xml 2009-08-05 06:38:16 UTC (rev 20761)
@@ -2,15 +2,20 @@
<description brief="A 2D navigation application for the PR2 robot platform.">
The 2D navigation application for the PR2 robot
</description>
- <version>0.1</version>
<author>Eitan ei...@wi...</author>
<license>BSD</license>
<review status="unreviewed" notes=""/>
<url>http://pr.willowgarage.com/wiki/2dnav_pr2_app</url>
- <depend stack="ros" version="0.5"/>
- <depend stack="ros-pkg_common" version="0.1"/>
- <depend stack="ros-pkg_visualization_core" version="0.1"/>
- <depend stack="navigation" version="0.1"/>
+
+ <depend stack="audio"/> <!-- backup_safetysound -->
+ <depend stack="geometry"/> <!-- tf, bullet -->
+ <depend stack="semantic_mapping"/> <!-- semantic_point_annotator -->
+ <depend stack="mechanism"/> <!-- mechanism_control -->
+ <depend stack="controllers"/> <!-- pr2_mechanism_controllers -->
+ <depend stack="common"/> <!-- laser_scan, robot_actions -->
+ <depend stack="estimation"/> <!-- robot_pose_ekf -->
+ <depend stack="navigation"/> <!-- amcl, map_server, move_base, nav_robot_actions -->
+ <depend stack="common_msgs"/> <!-- robot_msgs -->
+
</stack>
-
Added: pkg/trunk/audio/CMakeLists.txt
===================================================================
--- pkg/trunk/audio/CMakeLists.txt (rev 0)
+++ pkg/trunk/audio/CMakeLists.txt 2009-08-05 06:38:16 UTC (rev 20761)
@@ -0,0 +1,21 @@
+cmake_minimum_required(VERSION 2.4.6)
+include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
+
+set(ROSPACK_MAKEDIST true)
+
+# Append to CPACK_SOURCE_IGNORE_FILES a semicolon-separated list of
+# directories (or patterns, but directories should suffice) that should
+# be excluded from the distro. This is not the place to put things that
+# should be ignored everywhere, like "build" directories; that happens in
+# rosbuild/rosbuild.cmake. Here should be listed packages that aren't
+# ready for inclusion in a distro.
+#
+# This list is combined with the list in rosbuild/rosbuild.cmake. Note
+# that CMake 2.6 may be required to ensure that the two lists are combined
+# properly. CMake 2.4 seems to have unpredictable scoping rules for such
+# variables.
+#list(APPEND CPACK_SOURCE_IGNORE_FILES /core/experimental)
+
+rosbuild(audio 0.1.0)
+# After next ROS release, change to new macro
+#rosbuild_make_distribution(0.1.0)
Added: pkg/trunk/audio/Makefile
===================================================================
--- pkg/trunk/audio/Makefile (rev 0)
+++ pkg/trunk/audio/Makefile 2009-08-05 06:38:16 UTC (rev 20761)
@@ -0,0 +1 @@
+include $(shell rospack find mk)/cmake_stack.mk
Modified: pkg/trunk/audio/stack.xml
===================================================================
--- pkg/trunk/audio/stack.xml 2009-08-05 06:33:38 UTC (rev 20760)
+++ pkg/trunk/audio/stack.xml 2009-08-05 06:38:16 UTC (rev 20761)
@@ -2,12 +2,14 @@
<description brief="audio packages from ros-pkg">
These are audio-related packages.
</description>
- <version>0.1</version>
<author>Blaise Gassend bl...@wi...</author>
<license>BSD</license>
<review status="unreviewed" notes=""/>
<url>http://pr.willowgarage.com/wiki/audio</url>
- <depend stack="ros" version="0.5"/>
+
+ <depend stack="sound_drivers"/> <!-- sound_play -->
+ <depend stack="ros"/> <!-- rospy, roscpp -->
+ <depend stack="common_msgs"/> <!-- robot_msgs -->
+
</stack>
-
Added: pkg/trunk/calibration/CMakeLists.txt
===================================================================
--- pkg/trunk/calibration/CMakeLists.txt (rev 0)
+++ pkg/trunk/calibration/CMakeLists.txt 2009-08-05 06:38:16 UTC (rev 20761)
@@ -0,0 +1,21 @@
+cmake_minimum_required(VERSION 2.4.6)
+include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
+
+set(ROSPACK_MAKEDIST true)
+
+# Append to CPACK_SOURCE_IGNORE_FILES a semicolon-separated list of
+# directories (or patterns, but directories should suffice) that should
+# be excluded from the distro. This is not the place to put things that
+# should be ignored everywhere, like "build" directories; that happens in
+# rosbuild/rosbuild.cmake. Here should be listed packages that aren't
+# ready for inclusion in a distro.
+#
+# This list is combined with the list in rosbuild/rosbuild.cmake. Note
+# that CMake 2.6 may be required to ensure that the two lists are combined
+# properly. CMake 2.4 seems to have unpredictable scoping rules for such
+# variables.
+#list(APPEND CPACK_SOURCE_IGNORE_FILES /core/experimental)
+
+rosbuild(calibration 0.1.0)
+# After next ROS release, change to new macro
+#rosbuild_make_distribution(0.1.0)
Added: pkg/trunk/calibration/Makefile
===================================================================
--- pkg/trunk/calibration/Makefile (rev 0)
+++ pkg/trunk/calibration/Makefile 2009-08-05 06:38:16 UTC (rev 20761)
@@ -0,0 +1 @@
+include $(shell rospack find mk)/cmake_stack.mk
Modified: pkg/trunk/calibration/stack.xml
===================================================================
--- pkg/trunk/calibration/stack.xml 2009-08-05 06:33:38 UTC (rev 20760)
+++ pkg/trunk/calibration/stack.xml 2009-08-05 06:38:16 UTC (rev 20761)
@@ -2,12 +2,21 @@
<description brief="calibration packages from ros-pkg">
These are calibration-related packages.
</description>
- <version>0.1</version>
<author>Vijay Pradeep vpr...@wi...</author>
<license>BSD</license>
<review status="unreviewed" notes=""/>
<url>http://pr.willowgarage.com/wiki/calibration</url>
- <depend stack="ros" version="0.5"/>
+
+ <depend stack="controllers"/> <!-- pr2_mechanism_controllers, pr2_mechanism_controllers, pr2_mechanism_controllers, pr2_mechanism_controllers -->
+ <depend stack="opencv"/> <!-- opencv_latest -->
+ <depend stack="geometry"/> <!-- kdl, tf, kdl, tf -->
+ <depend stack="drivers"/> <!-- dense_laser_assembler, phase_space -->
+ <depend stack="mechanism"/> <!-- mechanism_model, hardware_interface, mechanism_msgs, mechanism_msgs, mechanism_msgs, mechanism_msgs, mechanism_msgs -->
+ <depend stack="util"/> <!-- topic_synchronizer, topic_synchronizer -->
+ <depend stack="openrave_planning"/> <!-- openraveros -->
+ <depend stack="common"/> <!-- tinyxml, robot_actions, laser_scan, robot_actions, manipulation_msgs, manipulation_msgs -->
+ <depend stack="ros"/> <!-- std_msgs, rospy, rosoct, std_msgs, std_msgs, message_filters, roscpp, message_filters, std_msgs, std_msgs, std_msgs, rosrecord, roscpp, std_msgs, std_msgs, rospy -->
+ <depend stack="common_msgs"/> <!-- geometry_msgs, robot_msgs, sensor_msgs, robot_msgs, sensor_msgs, robot_msgs, sensor_msgs, robot_msgs, sensor_msgs, robot_msgs, sensor_msgs, robot_msgs, geometry_msgs, sensor_msgs, robot_msgs, robot_msgs, robot_msgs, diagnostic_msgs -->
+
</stack>
-
Added: pkg/trunk/controllers/CMakeLists.txt
===================================================================
--- pkg/trunk/controllers/CMakeLists.txt (rev 0)
+++ pkg/trunk/controllers/CMakeLists.txt 2009-08-05 06:38:16 UTC (rev 20761)
@@ -0,0 +1,21 @@
+cmake_minimum_required(VERSION 2.4.6)
+include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
+
+set(ROSPACK_MAKEDIST true)
+
+# Append to CPACK_SOURCE_IGNORE_FILES a semicolon-separated list of
+# directories (or patterns, but directories should suffice) that should
+# be excluded from the distro. This is not the place to put things that
+# should be ignored everywhere, like "build" directories; that happens in
+# rosbuild/rosbuild.cmake. Here should be listed packages that aren't
+# ready for inclusion in a distro.
+#
+# This list is combined with the list in rosbuild/rosbuild.cmake. Note
+# that CMake 2.6 may be required to ensure that the two lists are combined
+# properly. CMake 2.4 seems to have unpredictable scoping rules for such
+# variables.
+#list(APPEND CPACK_SOURCE_IGNORE_FILES /core/experimental)
+
+rosbuild(controllers 0.1.0)
+# After next ROS release, change to new macro
+#rosbuild_make_distribution(0.1.0)
Added: pkg/trunk/controllers/Makefile
===================================================================
--- pkg/trunk/controllers/Makefile (rev 0)
+++ pkg/trunk/controllers/Makefile 2009-08-05 06:38:16 UTC (rev 20761)
@@ -0,0 +1 @@
+include $(shell rospack find mk)/cmake_stack.mk
Modified: pkg/trunk/controllers/stack.xml
===================================================================
--- pkg/trunk/controllers/stack.xml 2009-08-05 06:33:38 UTC (rev 20760)
+++ pkg/trunk/controllers/stack.xml 2009-08-05 06:38:16 UTC (rev 20761)
@@ -2,12 +2,18 @@
<description brief="controller packages from ros-pkg">
These are robot controllers.
</description>
- <version>0.1</version>
<author>Melonee Wise mw...@wi...</author>
<license>BSD</license>
<review status="unreviewed" notes=""/>
<url>http://pr.willowgarage.com/wiki/controllers</url>
- <depend stack="ros" version="0.5"/>
+
+ <depend stack="geometry"/> <!-- tf, kdl, angles, eigen, eigen, tf, angles, angles -->
+ <depend stack="mechanism"/> <!-- mechanism_model, mechanism_model, kdl_parser, mechanism_model, mechanism_control, hardware_interface, mechanism_control, mechanism_model, mechanism_msgs -->
+ <depend stack="util"/> <!-- realtime_tools, realtime_tools, realtime_tools, realtime_tools, trajectory -->
+ <depend stack="common"/> <!-- tinyxml, manipulation_msgs, filters, tinyxml, robot_actions, filters, manipulation_msgs -->
+ <depend stack="ros"/> <!-- rospy, rospy, rospy, roscpp, std_msgs, rosconsole, roscpp, roscpp, roscpp, rospy, std_msgs, rosconsole -->
+ <depend stack="visualization_core"/> <!-- visualization_msgs, visualization_msgs -->
+ <depend stack="common_msgs"/> <!-- robot_msgs, robot_srvs, robot_msgs, robot_msgs, robot_srvs, robot_srvs, robot_msgs, geometry_msgs, robot_msgs, geometry_msgs -->
+
</stack>
-
Added: pkg/trunk/drivers/CMakeLists.txt
===================================================================
--- pkg/trunk/drivers/CMakeLists.txt (rev 0)
+++ pkg/trunk/drivers/CMakeLists.txt 2009-08-05 06:38:16 UTC (rev 20761)
@@ -0,0 +1,21 @@
+cmake_minimum_required(VERSION 2.4.6)
+include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
+
+set(ROSPACK_MAKEDIST true)
+
+# Append to CPACK_SOURCE_IGNORE_FILES a semicolon-separated list of
+# directories (or patterns, but directories should suffice) that should
+# be excluded from the distro. This is not the place to put things that
+# should be ignored everywhere, like "build" directories; that happens in
+# rosbuild/rosbuild.cmake. Here should be listed packages that aren't
+# ready for inclusion in a distro.
+#
+# This list is combined with the list in rosbuild/rosbuild.cmake. Note
+# that CMake 2.6 may be required to ensure that the two lists are combined
+# properly. CMake 2.4 seems to have unpredictable scoping rules for such
+# variables.
+#list(APPEND CPACK_SOURCE_IGNORE_FILES /core/experimental)
+
+rosbuild(drivers 0.1.0)
+# After next ROS release, change to new macro
+#rosbuild_make_distribution(0.1.0)
Added: pkg/trunk/drivers/Makefile
===================================================================
--- pkg/trunk/drivers/Makefile (rev 0)
+++ pkg/trunk/drivers/Makefile 2009-08-05 06:38:16 UTC (rev 20761)
@@ -0,0 +1 @@
+include $(shell rospack find mk)/cmake_stack.mk
Modified: pkg/trunk/drivers/stack.xml
===================================================================
--- pkg/trunk/drivers/stack.xml 2009-08-05 06:33:38 UTC (rev 20760)
+++ pkg/trunk/drivers/stack.xml 2009-08-05 06:38:16 UTC (rev 20761)
@@ -2,13 +2,25 @@
<description brief="driver packages from ros-pkg">
These are hardware drivers.
</description>
- <version>0.1</version>
<author>Blaise Gassend bl...@wi...</author>
<license>BSD</license>
<review status="unreviewed" notes=""/>
<url>http://pr.willowgarage.com/wiki/drivers</url>
- <depend stack="ros" version="0.5"/>
- <depend stack="common" version="0.1"/>
+
+ <depend stack="util"/> <!-- image_utils, realtime_tools, image_utils -->
+ <depend stack="camera_drivers"/> <!-- prosilica_cam, dcam -->
+ <depend stack="calibration"/> <!-- calibration_msgs, mocap_msgs, mocap_msgs -->
+ <depend stack="geometry"/> <!-- tf, tf, angles, bullet, tf, tf -->
+ <depend stack="hardware_test"/> <!-- diagnostic_updater, diagnostic_updater, diagnostic_updater, diagnostic_updater, self_test, self_test -->
+ <depend stack="3rdparty"/> <!-- player -->
+ <depend stack="mechanism"/> <!-- mechanism_msgs, hardware_interface, mechanism_control, mechanism_model, mechanism_msgs, mechanism_msgs -->
+ <depend stack="controllers"/> <!-- pr2_mechanism_controllers, robot_mechanism_controllers, pr2_mechanism_controllers, dynamic_verification_controllers, pr2_mechanism_controllers, robot_mechanism_controllers, robot_mechanism_controllers -->
+ <depend stack="opencv"/> <!-- opencv_latest, opencv_latest, opencv_latest -->
+ <depend stack="simulators"/> <!-- gazebo, opende -->
+ <depend stack="common"/> <!-- laser_scan, tinyxml -->
+ <depend stack="driver_common"/> <!-- driver_base, dynamic_reconfigure -->
+ <depend stack="ros"/> <!-- std_msgs, roscpp, rospy, std_msgs, rostest, std_msgs, roscpp, rospy, std_msgs, std_msgs, roscpp, roscpp, std_msgs, message_filters, rospy, roscpp, std_msgs, roscpp, std_msgs, roscpp, std_msgs, rospy, rospy, roscpp, roscpp, std_msgs, roscpp, rospy, std_msgs, roscpp, std_msgs, roscpp, std_msgs -->
+ <depend stack="common_msgs"/> <!-- sensor_msgs, robot_msgs, geometry_msgs, robot_msgs, sensor_msgs, robot_msgs, sensor_msgs, sensor_msgs, robot_msgs, diagnostic_msgs, robot_msgs, robot_msgs, sensor_msgs, sensor_msgs, diagnostic_msgs, robot_msgs, robot_msgs, diagnostic_msgs, robot_msgs, sensor_msgs, robot_msgs, sensor_msgs -->
+
</stack>
-
Added: pkg/trunk/highlevel/CMakeLists.txt
===================================================================
--- pkg/trunk/highlevel/CMakeLists.txt (rev 0)
+++ pkg/trunk/highlevel/CMakeLists.txt 2009-08-05 06:38:16 UTC (rev 20761)
@@ -0,0 +1,21 @@
+cmake_minimum_required(VERSION 2.4.6)
+include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
+
+set(ROSPACK_MAKEDIST true)
+
+# Append to CPACK_SOURCE_IGNORE_FILES a semicolon-separated list of
+# directories (or patterns, but directories should suffice) that should
+# be excluded from the distro. This is not the place to put things that
+# should be ignored everywhere, like "build" directories; that happens in
+# rosbuild/rosbuild.cmake. Here should be listed packages that aren't
+# ready for inclusion in a distro.
+#
+# This list is combined with the list in rosbuild/rosbuild.cmake. Note
+# that CMake 2.6 may be required to ensure that the two lists are combined
+# properly. CMake 2.4 seems to have unpredictable scoping rules for such
+# variables.
+#list(APPEND CPACK_SOURCE_IGNORE_FILES /core/experimental)
+
+rosbuild(highlevel 0.1.0)
+# After next ROS release, change to new macro
+#rosbuild_make_distribution(0.1.0)
Added: pkg/trunk/highlevel/Makefile
===================================================================
--- pkg/trunk/highlevel/Makefile (rev 0)
+++ pkg/trunk/highlevel/Makefile 2009-08-05 06:38:16 UTC (rev 20761)
@@ -0,0 +1 @@
+include $(shell rospack find mk)/cmake_stack.mk
Modified: pkg/trunk/highlevel/stack.xml
===================================================================
--- pkg/trunk/highlevel/stack.xml 2009-08-05 06:33:38 UTC (rev 20760)
+++ pkg/trunk/highlevel/stack.xml 2009-08-05 06:38:16 UTC (rev 20761)
@@ -5,13 +5,25 @@
map" or "open the door in front of you" or "pick up the stapler in
front of you."
</description>
- <version>0.1</version>
<author>Conor McGann mc...@wi...</author>
<license>BSD</license>
<review status="unreviewed" notes=""/>
<url>http://pr.willowgarage.com/wiki/highlevel</url>
- <depend stack="ros" version="0.5"/>
-</stack>
+ <depend stack="util"/> <!-- or_robot_self_filter -->
+ <depend stack="estimation"/> <!-- robot_pose_ekf -->
+ <depend stack="geometry"/> <!-- kdl, tf, kdl, tf, tf, tf, tf -->
+ <depend stack="navigation"/> <!-- nav_robot_actions, nav_robot_actions, costmap_2d, base_local_planner, fake_localization, map_server, nav_view, move_base, amcl, map_server, fake_localization, nav_view, nav_robot_actions -->
+ <depend stack="semantic_mapping"/> <!-- plug_onbase_detector, door_handle_detector, point_cloud_mapping -->
+ <depend stack="mechanism"/> <!-- mechanism_msgs -->
+ <depend stack="controllers"/> <!-- pr2_mechanism_controllers, pr2_mechanism_controllers, robot_mechanism_controllers, pr2_mechanism_controllers -->
+ <depend stack="simulators"/> <!-- stage, stage -->
+ <depend stack="common"/> <!-- robot_actions, manipulation_srvs, robot_actions, robot_actions, manipulation_msgs, robot_actions, laser_scan, robot_actions, robot_actions, robot_actions, python_actions -->
+ <depend stack="ros"/> <!-- rospy, roslib, roscpp, rosconsole, std_msgs, roslisp, std_msgs, std_srvs, roscpp, std_msgs, rospy, std_msgs, roscpp, std_msgs, roscpp, gtest, rosrecord, roscpp, std_msgs, roscpp, roscpp, std_msgs, roscpp, roslib -->
+ <depend stack="visualization_core"/> <!-- visualization_msgs, visualization_msgs -->
+ <depend stack="vision"/> <!-- people -->
+ <depend stack="common_msgs"/> <!-- robot_msgs, robot_msgs, robot_srvs, robot_msgs, robot_msgs, diagnostic_msgs, robot_srvs, nav_msgs, geometry_msgs, geometry_msgs, robot_msgs, robot_srvs, robot_msgs, geometry_msgs, robot_msgs, robot_srvs -->
+
+</stack>
Added: pkg/trunk/openrave_planning/CMakeLists.txt
===================================================================
--- pkg/trunk/openrave_planning/CMakeLists.txt (rev 0)
+++ pkg/trunk/openrave_planning/CMakeLists.txt 2009-08-05 06:38:16 UTC (rev 20761)
@@ -0,0 +1,21 @@
+cmake_minimum_required(VERSION 2.4.6)
+include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
+
+set(ROSPACK_MAKEDIST true)
+
+# Append to CPACK_SOURCE_IGNORE_FILES a semicolon-separated list of
+# directories (or patterns, but directories should suffice) that should
+# be excluded from the distro. This is not the place to put things that
+# should be ignored everywhere, like "build" directories; that happens in
+# rosbuild/rosbuild.cmake. Here should be listed packages that aren't
+# ready for inclusion in a distro.
+#
+# This list is combined with the list in rosbuild/rosbuild.cmake. Note
+# that CMake 2.6 may be required to ensure that the two lists are combined
+# properly. CMake 2.4 seems to have unpredictable scoping rules for such
+# variables.
+#list(APPEND CPACK_SOURCE_IGNORE_FILES /core/experimental)
+
+rosbuild(openrave_planning 0.1.0)
+# After next ROS release, change to new macro
+#rosbuild_make_distribution(0.1.0)
Added: pkg/trunk/openrave_planning/Makefile
===================================================================
--- pkg/trunk/openrave_planning/Makefile (rev 0)
+++ pkg/trunk/openrave_planning/Makefile 2009-08-05 06:38:16 UTC (rev 20761)
@@ -0,0 +1 @@
+include $(shell rospack find mk)/cmake_stack.mk
Modified: pkg/trunk/openrave_planning/stack.xml
===================================================================
--- pkg/trunk/openrave_planning/stack.xml 2009-08-05 06:33:38 UTC (rev 20760)
+++ pkg/trunk/openrave_planning/stack.xml 2009-08-05 06:38:16 UTC (rev 20761)
@@ -2,14 +2,19 @@
<description brief="openrave packages from cmu-ros-pkg">
These are OpenRAVE-related packages.
</description>
- <version>0.1</version>
<author>Rosen Diankov rdi...@cs...</author>
<license>BSD</license>
<review status="unreviewed" notes=""/>
<url>http://pr.willowgarage.com/wiki/openrave</url>
- <depend stack="ros" version="0.5"/>
- <depend stack="common" version="...
[truncated message content] |
|
From: <wa...@us...> - 2009-08-05 17:18:36
|
Revision: 20786
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=20786&view=rev
Author: wattsk
Date: 2009-08-05 17:18:24 +0000 (Wed, 05 Aug 2009)
Log Message:
-----------
Removing old dynamic verification controllers and analysis packages
Modified Paths:
--------------
pkg/trunk/drivers/simulator/gazebo_plugin/manifest.xml
pkg/trunk/pr2/qualification/tests/wrist_test/hysteresis_wrist_flex.launch
pkg/trunk/pr2/qualification/tests/wrist_test/hysteresis_wrist_flex.xml
pkg/trunk/stacks/pr2/pr2_etherCAT/manifest.xml
Added Paths:
-----------
pkg/trunk/pr2/invent_client/mainpage.dox
Removed Paths:
-------------
pkg/trunk/controllers/qualification_controllers/dynamic_verification_controllers/bin/
pkg/trunk/controllers/qualification_controllers/dynamic_verification_controllers/include/
pkg/trunk/controllers/qualification_controllers/dynamic_verification_controllers/src/
pkg/trunk/controllers/qualification_controllers/dynamic_verification_controllers/srv/
pkg/trunk/pr2/dynamic_verification/
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/manifest.xml
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/manifest.xml 2009-08-05 17:00:18 UTC (rev 20785)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/manifest.xml 2009-08-05 17:18:24 UTC (rev 20786)
@@ -12,7 +12,6 @@
<depend package="robot_mechanism_controllers"/>
<depend package="pr2_mechanism_controllers"/>
<depend package="experimental_controllers"/>
- <depend package="dynamic_verification_controllers"/>
<depend package="roscpp"/>
<depend package="prosilica_cam"/>
<depend package="rospy"/>
Added: pkg/trunk/pr2/invent_client/mainpage.dox
===================================================================
--- pkg/trunk/pr2/invent_client/mainpage.dox (rev 0)
+++ pkg/trunk/pr2/invent_client/mainpage.dox 2009-08-05 17:18:24 UTC (rev 20786)
@@ -0,0 +1,110 @@
+/**
+\mainpage
+\htmlinclude manifest.html
+
+\b my_package is ... In addition to providing an overview of your package,
+this is the section where the specification and design/architecture
+should be detailed. While the original specification may be done on the
+wiki, it should be transferred here once your package starts to take shape.
+You can then link to this documentation page from the Wiki.
+
+
+\section codeapi Code API
+
+Provide links to specific auto-generated API documentation within your
+package that is of particular interest to a reader. Doxygen will
+document pretty much every part of your code, so do your best here to
+point the reader to the actual API.
+
+If your codebase is fairly large or has different sets of APIs, you
+should use the doxygen 'group' tag to keep these APIs together. For
+example, the roscpp documentation has 'libros' and 'botherder' groups
+so that those can be viewed separately. The rospy documentation
+similarly has a 'client-api' group that pulls together APIs for a
+Client API page.
+
+
+\section rosapi ROS API
+
+Every ROS name in your code must be documented. Names are very
+important in ROS because they are the API to nodes and services. They
+are also capable of being remapped on the command-line, so it is VERY
+IMPORTANT THAT YOU LIST NAMES AS THEY APPEAR IN THE CODE. It is also
+important that you write your code so that the names can be easily
+remapped.
+
+List of nodes:
+- \b node_name1
+- \b node_name2
+
+
+<!-- START: copy for each node -->
+
+<hr>
+
+\subsection node_name node_name
+
+node_name does (provide a basic description of your node)
+
+\subsubsection Usage
+\verbatim
+$ node_type1 [standard ROS args]
+\endverbatim
+
+\par Example
+
+\verbatim
+$ node_type1
+\endverbatim
+
+
+\subsubsection topics ROS topics
+
+Subscribes to:
+- \b "in": [std_msgs/FooType] description of in
+
+Publishes to:
+- \b "out": [std_msgs/FooType] description of out
+
+\subsubsection parameters ROS parameters
+
+Reads the following parameters from the parameter server
+
+- \b "~param_name" : \b [type] description of param_name
+- \b "~my_param" : \b [string] description of my_param
+
+Sets the following parameters on the parameter server
+
+- \b "~param_name" : \b [type] description of param_name
+
+
+\subsubsection services ROS services
+- \b "foo_service": [std_srvs/FooType] description of foo_service
+
+
+<!-- END: copy for each node -->
+
+\section commandline Command-line tools
+
+This section is a catch-all for any additional tools that your package
+provides or uses that may be of use to the reader. For example:
+
+- tools/scripts (e.g. rospack, roscd)
+- roslaunch .launch files
+- xmlparam files
+
+\subsection script_name script_name
+
+Description of what this script/file does.
+
+\subsubsection Usage
+\verbatim
+$ ./script_name [args]
+\endverbatim
+
+\par Example
+
+\verbatim
+$ ./script_name foo bar
+\endverbatim
+*/
\ No newline at end of file
Modified: pkg/trunk/pr2/qualification/tests/wrist_test/hysteresis_wrist_flex.launch
===================================================================
--- pkg/trunk/pr2/qualification/tests/wrist_test/hysteresis_wrist_flex.launch 2009-08-05 17:00:18 UTC (rev 20785)
+++ pkg/trunk/pr2/qualification/tests/wrist_test/hysteresis_wrist_flex.launch 2009-08-05 17:18:24 UTC (rev 20786)
@@ -1,5 +1,5 @@
<launch>
-<!-- Moved qualification node up so plotting function can be ready to recieve -->
+
<node pkg="qualification" type="hysteresis_sinesweep_plot.py" />
<node pkg="mechanism_control" type="spawner.py" args="$(find qualification)/tests/wrist_test/hysteresis_wrist_flex.xml" />
</launch>
Modified: pkg/trunk/pr2/qualification/tests/wrist_test/hysteresis_wrist_flex.xml
===================================================================
--- pkg/trunk/pr2/qualification/tests/wrist_test/hysteresis_wrist_flex.xml 2009-08-05 17:00:18 UTC (rev 20785)
+++ pkg/trunk/pr2/qualification/tests/wrist_test/hysteresis_wrist_flex.xml 2009-08-05 17:18:24 UTC (rev 20786)
@@ -1,4 +1,3 @@
-<!--controllers-->
<controller name="test_controller" type="HysteresisControllerNode">
<joint name="r_wrist_flex_joint">
<pid p="2" d="0" i="10" iClamp="0.1" />
@@ -6,9 +5,4 @@
</joint>
</controller>
- <!--controller name="wrist_roll_controller" type="JointPositionControllerNode">
- <joint name="r_wrist_roll_joint">
- <pid p="4" i="0.5" d="0" iClamp="1.0" />
- </joint>
- </controller>
-</controllers-->
+
Modified: pkg/trunk/stacks/pr2/pr2_etherCAT/manifest.xml
===================================================================
--- pkg/trunk/stacks/pr2/pr2_etherCAT/manifest.xml 2009-08-05 17:00:18 UTC (rev 20785)
+++ pkg/trunk/stacks/pr2/pr2_etherCAT/manifest.xml 2009-08-05 17:18:24 UTC (rev 20786)
@@ -13,7 +13,6 @@
<depend package="robot_mechanism_controllers"/>
<depend package="pr2_mechanism_controllers"/>
<depend package="joint_qualification_controllers"/>
- <depend package="dynamic_verification_controllers" />
<depend package="realtime_tools"/>
<depend package="diagnostic_msgs"/>
<depend package="experimental_controllers"/>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
Thanks for helping keep SourceForge clean.
X