Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[23393] pkg/trunk
|
From: <mee...@us...> - 2009-08-31 18:22:29
|
Revision: 23393
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=23393&view=rev
Author: meeussen
Date: 2009-08-31 18:22:20 +0000 (Mon, 31 Aug 2009)
Log Message:
-----------
replace pr2_mechanism_msgs::JointStates by new non-pr2-specific sensor_msgs::JointState. Door test passes
Modified Paths:
--------------
pkg/trunk/calibration_experimental/joint_calibration_monitor/manifest.xml
pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/head_position_controller.h
pkg/trunk/controllers/pr2_mechanism_controllers/src/head_position_controller.cpp
pkg/trunk/motion_planning/planning_environment/include/planning_environment/monitors/kinematic_model_state_monitor.h
pkg/trunk/motion_planning/planning_environment/manifest.xml
pkg/trunk/motion_planning/planning_environment/src/monitors/kinematic_model_state_monitor.cpp
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/manifest.xml
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_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/pr2/teleop/teleop_head/manifest.xml
pkg/trunk/pr2/teleop/teleop_head/teleop_head.cpp
pkg/trunk/pr2/teleop/teleop_head/teleop_head_keyboard.cpp
pkg/trunk/pr2/teleop/teleop_pr2/manifest.xml
pkg/trunk/pr2/teleop/teleop_pr2/teleop_pr2.cpp
pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/head_servoing_controller.h
pkg/trunk/sandbox/experimental_controllers/manifest.xml
pkg/trunk/sandbox/experimental_controllers/src/head_servoing_controller.cpp
pkg/trunk/sandbox/geometric_blur/.build_version
pkg/trunk/sandbox/geometric_blur/.rosgcov_files
pkg/trunk/sandbox/hanoi/manifest.xml
pkg/trunk/sandbox/hanoi/src/Hanoi.cpp
pkg/trunk/sandbox/move_arm_tools/manifest.xml
pkg/trunk/sandbox/robot_voxelizer/include/robot_voxelizer/robot_voxelizer.h
pkg/trunk/sandbox/robot_voxelizer/manifest.xml
pkg/trunk/sandbox/robot_voxelizer/src/robot_voxelizer.cpp
pkg/trunk/sandbox/teleop_anti_collision/manifest.xml
pkg/trunk/sandbox/teleop_anti_collision/src/teleop_goal_projection.cpp
pkg/trunk/stacks/calibration/joint_states_settler/include/joint_states_settler/deflated_joint_states.h
pkg/trunk/stacks/calibration/joint_states_settler/include/joint_states_settler/joint_states_deflater.h
pkg/trunk/stacks/calibration/joint_states_settler/include/joint_states_settler/joint_states_settler.h
pkg/trunk/stacks/calibration/joint_states_settler/manifest.xml
pkg/trunk/stacks/calibration/joint_states_settler/src/joint_states_deflater.cpp
pkg/trunk/stacks/calibration/joint_states_settler/src/joint_states_settler.cpp
pkg/trunk/stacks/calibration/joint_states_settler/src/joint_states_settler_node.cpp
pkg/trunk/stacks/pr2_common/pr2_msgs/manifest.xml
pkg/trunk/stacks/pr2_mechanism/pr2_mechanism_control/include/pr2_mechanism_control/mechanism_control.h
pkg/trunk/stacks/pr2_mechanism/pr2_mechanism_control/src/mechanism_control.cpp
pkg/trunk/stacks/pr2_simulator/gazebo_plugin/manifest.xml
pkg/trunk/stacks/robot_model/robot_state_publisher/include/robot_state_publisher/joint_state_listener.h
pkg/trunk/stacks/robot_model/robot_state_publisher/manifest.xml
pkg/trunk/stacks/robot_model/robot_state_publisher/src/joint_state_listener.cpp
pkg/trunk/stacks/robot_model/robot_state_publisher/src/state_publisher.cpp
pkg/trunk/stacks/robot_model/stack.xml
Added Paths:
-----------
pkg/trunk/stacks/common_msgs/sensor_msgs/msg/JointState.msg
Removed Paths:
-------------
pkg/trunk/stacks/pr2_mechanism/pr2_mechanism_msgs/msg/JointStates.msg
Modified: pkg/trunk/calibration_experimental/joint_calibration_monitor/manifest.xml
===================================================================
--- pkg/trunk/calibration_experimental/joint_calibration_monitor/manifest.xml 2009-08-31 18:17:17 UTC (rev 23392)
+++ pkg/trunk/calibration_experimental/joint_calibration_monitor/manifest.xml 2009-08-31 18:22:20 UTC (rev 23393)
@@ -11,7 +11,4 @@
<!-- common_msgs -->
<depend package="diagnostic_msgs" />
- <!-- mechanism -->
- <depend package="pr2_mechanism_msgs" />
-
</package>
Modified: 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_position_controller.h 2009-08-31 18:17:17 UTC (rev 23392)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/head_position_controller.h 2009-08-31 18:22:20 UTC (rev 23393)
@@ -46,7 +46,7 @@
#include <tf/message_notifier.h>
#include <tf/transform_datatypes.h>
#include <robot_mechanism_controllers/joint_position_controller.h>
-#include <pr2_mechanism_msgs/JointStates.h>
+#include <sensor_msgs/JointState.h>
#include <geometry_msgs/PointStamped.h>
#include <boost/scoped_ptr.hpp>
@@ -73,7 +73,7 @@
mechanism::RobotState *robot_state_;
ros::Subscriber sub_command_;
- void command(const pr2_mechanism_msgs::JointStatesConstPtr& command_msg);
+ void command(const sensor_msgs::JointStateConstPtr& command_msg);
void pointHead(const tf::MessageNotifier<geometry_msgs::PointStamped>::MessagePtr& point_msg);
void pointFrameOnHead(const tf::MessageNotifier<geometry_msgs::PointStamped>::MessagePtr& point_msg);
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/head_position_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/head_position_controller.cpp 2009-08-31 18:17:17 UTC (rev 23392)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/head_position_controller.cpp 2009-08-31 18:22:20 UTC (rev 23393)
@@ -84,7 +84,7 @@
// subscribe to head commands
- sub_command_ = node_.subscribe<pr2_mechanism_msgs::JointStates>("command", 1, &HeadPositionController::command, this);
+ sub_command_ = node_.subscribe<sensor_msgs::JointState>("command", 1, &HeadPositionController::command, this);
point_head_notifier_.reset(new MessageNotifier<geometry_msgs::PointStamped>(tf_,
boost::bind(&HeadPositionController::pointHead, this, _1),
@@ -114,21 +114,30 @@
head_tilt_controller_.update();
}
-void HeadPositionController::command(const pr2_mechanism_msgs::JointStatesConstPtr& command_msg)
+void HeadPositionController::command(const sensor_msgs::JointStateConstPtr& command_msg)
{
-
- assert(command_msg->joints.size() == 2);
- if(command_msg->joints[0].name == head_pan_controller_.joint_state_->joint_->name_)
+ // do not use assert to check user input!
+
+ if (command_msg->name.size() != 2 || command_msg->position.size() != 2){
+ ROS_ERROR("Head servoing controller expected joint command of size 2");
+ return;
+ }
+ if (command_msg->name[0] == head_pan_controller_.joint_state_->joint_->name_ &&
+ command_msg->name[1] == head_tilt_controller_.joint_state_->joint_->name_)
{
- pan_out_ = command_msg->joints[0].position;
- tilt_out_ = command_msg->joints[1].position;
+ pan_out_ = command_msg->position[0];
+ tilt_out_ = command_msg->position[1];
}
+ else if (command_msg->name[1] == head_pan_controller_.joint_state_->joint_->name_ &&
+ command_msg->name[0] == head_tilt_controller_.joint_state_->joint_->name_)
+ {
+ pan_out_ = command_msg->position[1];
+ tilt_out_ = command_msg->position[0];
+ }
else
{
- pan_out_ = command_msg->joints[1].position;
- tilt_out_ = command_msg->joints[0].position;
+ ROS_ERROR("Head servoing controller received invalid joint command");
}
-
}
void HeadPositionController::pointHead(const tf::MessageNotifier<geometry_msgs::PointStamped>::MessagePtr& point_msg)
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-31 18:17:17 UTC (rev 23392)
+++ pkg/trunk/motion_planning/planning_environment/include/planning_environment/monitors/kinematic_model_state_monitor.h 2009-08-31 18:22:20 UTC (rev 23393)
@@ -41,7 +41,7 @@
#include <tf/transform_datatypes.h>
#include <tf/transform_listener.h>
#include <tf/message_notifier.h>
-#include <pr2_mechanism_msgs/JointStates.h>
+#include <sensor_msgs/JointState.h>
#include <mapping_msgs/AttachedObject.h>
#include <boost/bind.hpp>
#include <vector>
@@ -191,7 +191,7 @@
protected:
void setupRSM(void);
- void jointStateCallback(const pr2_mechanism_msgs::JointStatesConstPtr &jointState);
+ void jointStateCallback(const sensor_msgs::JointStateConstPtr &jointState);
void attachObjectCallback(const mapping_msgs::AttachedObjectConstPtr &attachedObject);
virtual bool attachObject(const mapping_msgs::AttachedObjectConstPtr &attachedObject);
Modified: pkg/trunk/motion_planning/planning_environment/manifest.xml
===================================================================
--- pkg/trunk/motion_planning/planning_environment/manifest.xml 2009-08-31 18:17:17 UTC (rev 23392)
+++ pkg/trunk/motion_planning/planning_environment/manifest.xml 2009-08-31 18:22:20 UTC (rev 23393)
@@ -15,7 +15,7 @@
<depend package="angles" />
<depend package="pr2_defs" />
<depend package="tabletop_msgs" />
- <depend package="pr2_mechanism_msgs" />
+ <depend package="sensor_msgs" />
<depend package="motion_planning_msgs" />
<depend package="mapping_msgs" />
<depend package="visualization_msgs" />
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-31 18:17:17 UTC (rev 23392)
+++ pkg/trunk/motion_planning/planning_environment/src/monitors/kinematic_model_state_monitor.cpp 2009-08-31 18:22:20 UTC (rev 23393)
@@ -124,41 +124,45 @@
stateMonitorStarted_ = false;
}
-void planning_environment::KinematicModelStateMonitor::jointStateCallback(const pr2_mechanism_msgs::JointStatesConstPtr &jointState)
+void planning_environment::KinematicModelStateMonitor::jointStateCallback(const sensor_msgs::JointStateConstPtr &jointState)
{
bool change = !haveJointState_;
static bool first_time = true;
double totalv = 0.0;
- unsigned int n = jointState->get_joints_size();
+ unsigned int n = jointState->get_name_size();
+ if (jointState->get_name_size() != jointState->get_position_size() || jointState->get_name_size() !=jointState->get_velocity_size()){
+ ROS_ERROR("Planning environment received invalid joint state");
+ return;
+ }
for (unsigned int i = 0 ; i < n ; ++i)
{
- planning_models::KinematicModel::Joint* joint = kmodel_->getJoint(jointState->joints[i].name);
+ planning_models::KinematicModel::Joint* joint = kmodel_->getJoint(jointState->name[i]);
if (joint)
{
if (joint->usedParams == 1)
{
- double pos = jointState->joints[i].position;
- double vel = jointState->joints[i].velocity;
+ double pos = jointState->position[i];
+ double vel = jointState->velocity[i];
totalv += vel * vel;
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, jointState->joints[i].name);
+ bool this_changed = robotState_->setParamsJoint(&pos, jointState->name[i]);
change = change || this_changed;
}
else
{
if (first_time)
- ROS_WARN("Incorrect number of parameters: %s (expected %d, had 1)", jointState->joints[i].name.c_str(), joint->usedParams);
+ ROS_WARN("Incorrect number of parameters: %s (expected %d, had 1)", jointState->name[i].c_str(), joint->usedParams);
}
}
else
{
if (first_time)
- ROS_ERROR("Unknown joint: %s", jointState->joints[i].name.c_str());
+ ROS_ERROR("Unknown joint: %s", jointState->name[i].c_str());
}
}
robotVelocity_ = sqrt(totalv);
Modified: 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/include/sbpl_arm_planner_node/sbpl_arm_planner_node.h 2009-08-31 18:17:17 UTC (rev 23392)
+++ pkg/trunk/motion_planning/planning_research/sbpl_arm_planner_node/include/sbpl_arm_planner_node/sbpl_arm_planner_node.h 2009-08-31 18:22:20 UTC (rev 23393)
@@ -185,7 +185,7 @@
pr2_mechanism_msgs::MechanismState mechanism_state_;
- pr2_mechanism_msgs::JointStates joint_states_;
+ sensor_msgs::JointState joint_states_;
tf::MessageNotifier<sensor_msgs::PointCloud> *point_cloud_notifier_;
@@ -265,7 +265,7 @@
void pointCloudCallback(const sensor_msgs::PointCloudConstPtr &point_cloud);
- void jointStatesCallback(const pr2_mechanism_msgs::JointStatesConstPtr &joint_states);
+ void jointStatesCallback(const sensor_msgs::JointStateConstPtr &joint_states);
void createOccupancyGrid();
Modified: pkg/trunk/motion_planning/planning_research/sbpl_arm_planner_node/manifest.xml
===================================================================
--- pkg/trunk/motion_planning/planning_research/sbpl_arm_planner_node/manifest.xml 2009-08-31 18:17:17 UTC (rev 23392)
+++ pkg/trunk/motion_planning/planning_research/sbpl_arm_planner_node/manifest.xml 2009-08-31 18:22:20 UTC (rev 23393)
@@ -13,7 +13,7 @@
<depend package="motion_planning_msgs" />
<depend package="planning_environment" />
<depend package="visualization_msgs" />
- <depend package="pr2_mechanism_msgs" />
+ <depend package="sensor_msgs" />
<depend package="tf" />
<!-- <depend package="gtest" /> -->
<depend package="pr2_ik" />
Modified: pkg/trunk/motion_planning/planning_research/sbpl_arm_planner_node/src/sbpl_arm_planner_node.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_research/sbpl_arm_planner_node/src/sbpl_arm_planner_node.cpp 2009-08-31 18:17:17 UTC (rev 23392)
+++ pkg/trunk/motion_planning/planning_research/sbpl_arm_planner_node/src/sbpl_arm_planner_node.cpp 2009-08-31 18:22:20 UTC (rev 23393)
@@ -436,9 +436,12 @@
}
}
-void SBPLArmPlannerNode::jointStatesCallback(const pr2_mechanism_msgs::JointStatesConstPtr &joint_states)
+void SBPLArmPlannerNode::jointStatesCallback(const sensor_msgs::JointStateConstPtr &joint_states)
{
- joint_states_ = *joint_states;
+ if (joint_states->get_name_size() != joint_states->get_position_size())
+ ROS_ERROR("SBPL door planner received invalid joint state");
+ else
+ joint_states_ = *joint_states;
}
/** \brief Fetch the SBPL collision map and publish it for debugging purposes in rviz */
Modified: 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/include/sbpl_door_planner_action/sbpl_door_planner_action.h 2009-08-31 18:17:17 UTC (rev 23392)
+++ pkg/trunk/motion_planning/planning_research/sbpl_door_planner_action/include/sbpl_door_planner_action/sbpl_door_planner_action.h 2009-08-31 18:22:20 UTC (rev 23393)
@@ -49,7 +49,7 @@
#include <mapping_msgs/CollisionMap.h>
#include <motion_planning_msgs/KinematicPath.h>
#include <pr2_mechanism_msgs/MechanismState.h>
-#include <pr2_mechanism_msgs/JointStates.h>
+#include <sensor_msgs/JointState.h>
// Costmap used for the map representation
#include <costmap_2d/costmap_2d_ros.h>
@@ -118,10 +118,10 @@
ros::Publisher pr2_ik_pub_;
ros::Publisher base_control_pub_;
tf::TransformListener tf_;
- pr2_mechanism_msgs::JointStates joint_states_;
+ sensor_msgs::JointState joint_states_;
motion_planning_msgs::KinematicPath robot_path_;
- void jointsCallback(const pr2_mechanism_msgs::JointStatesConstPtr &joint_states);
+ void jointsCallback(const sensor_msgs::JointStateConstPtr &joint_states);
// tf::TransformListener &tf_;
boost::shared_ptr<mpglue::CostmapAccessor> cm_access_;
Modified: pkg/trunk/motion_planning/planning_research/sbpl_door_planner_action/src/sbpl_door_planner_action.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_research/sbpl_door_planner_action/src/sbpl_door_planner_action.cpp 2009-08-31 18:17:17 UTC (rev 23392)
+++ pkg/trunk/motion_planning/planning_research/sbpl_door_planner_action/src/sbpl_door_planner_action.cpp 2009-08-31 18:22:20 UTC (rev 23393)
@@ -198,10 +198,13 @@
return true;
}
-void SBPLDoorPlanner::jointsCallback(const pr2_mechanism_msgs::JointStatesConstPtr &joint_states)
+void SBPLDoorPlanner::jointsCallback(const sensor_msgs::JointStateConstPtr &joint_states)
{
//this is a stupid way of doing things ?
- joint_states_ = *joint_states;
+ if (joint_states->get_name_size() != joint_states->get_position_size())
+ ROS_ERROR("SBPL door planner received invalid joint state");
+ else
+ joint_states_ = *joint_states;
}
bool SBPLDoorPlanner::removeDoor()
@@ -974,11 +977,11 @@
robot_path_.set_states_size(path_length);
// get start_state of the robot from joint_states topic
- for(i = 0; i < joint_states_.get_joints_size(); ++i)
+ for(i = 0; i < joint_states_.get_name_size(); ++i)
{
robot_path_.start_state[i].header = joint_states_.header;
- robot_path_.start_state[i].joint_name = joint_states_.joints[i].name;
- robot_path_.start_state[i].value[0] = joint_states_.joints[i].position;
+ robot_path_.start_state[i].joint_name = joint_states_.name[i];
+ robot_path_.start_state[i].value[0] = joint_states_.position[i];
}
// set names of arm joints in path
@@ -1029,12 +1032,12 @@
{
int nind = 0;
joint_angles->resize(num_joints_);
- for(unsigned int i = 0; i < joint_states_.get_joints_size(); i++)
+ for(unsigned int i = 0; i < joint_states_.get_name_size(); i++)
{
- if(joint_names_[nind].compare(joint_states_.joints[i].name) == 0)
+ if(joint_names_[nind].compare(joint_states_.name[i]) == 0)
{
- ROS_DEBUG("%s: %.3f",joint_states_.joints[i].name.c_str(), joint_states_.joints[i].position);
- joint_angles->at(nind) = joint_states_.joints[i].position;
+ ROS_DEBUG("%s: %.3f",joint_states_.name[i].c_str(), joint_states_.position[i]);
+ joint_angles->at(nind) = joint_states_.position[i];
nind++;
}
if(nind == num_joints_)
Modified: pkg/trunk/pr2/teleop/teleop_head/manifest.xml
===================================================================
--- pkg/trunk/pr2/teleop/teleop_head/manifest.xml 2009-08-31 18:17:17 UTC (rev 23392)
+++ pkg/trunk/pr2/teleop/teleop_head/manifest.xml 2009-08-31 18:22:20 UTC (rev 23393)
@@ -7,7 +7,7 @@
<license>BSD</license>
<review status="na" notes="" />
<depend package="joy" />
- <depend package="pr2_mechanism_msgs" />
+ <depend package="sensor_msgs" />
<depend package="pr2_default_controllers" />
<depend package="roscpp" />
</package>
Modified: pkg/trunk/pr2/teleop/teleop_head/teleop_head.cpp
===================================================================
--- pkg/trunk/pr2/teleop/teleop_head/teleop_head.cpp 2009-08-31 18:17:17 UTC (rev 23392)
+++ pkg/trunk/pr2/teleop/teleop_head/teleop_head.cpp 2009-08-31 18:22:20 UTC (rev 23393)
@@ -36,8 +36,7 @@
#include "ros/ros.h"
#include "joy/Joy.h"
-#include "pr2_mechanism_msgs/JointState.h"
-#include "pr2_mechanism_msgs/JointStates.h"
+#include "sensor_msgs/JointState.h"
class TeleopHead
{
@@ -80,7 +79,7 @@
ROS_DEBUG("deadman_button: %d\n", deadman_button);
- head_pub_ = n_.advertise<pr2_mechanism_msgs::JointStates>("head_controller/command", 1);
+ head_pub_ = n_.advertise<sensor_msgs::JointState>("head_controller/command", 1);
joy_sub_ = n_.subscribe("joy", 10, &TeleopHead::joy_cb, this);
}
@@ -113,33 +112,26 @@
{
if (deadman_)
{
- pr2_mechanism_msgs::JointState joint_cmd ;
- pr2_mechanism_msgs::JointStates joint_cmds;
-
- joint_cmd.name ="head_pan_joint";
- joint_cmd.position = req_pan;
- joint_cmds.joints.push_back(joint_cmd);
- joint_cmd.name="head_tilt_joint";
- joint_cmd.position = req_tilt;
- joint_cmds.joints.push_back(joint_cmd);
-
+ sensor_msgs::JointState joint_cmds;
+ joint_cmds.set_name_size(2);
+ joint_cmds.set_position_size(2);
+ joint_cmds.name[0] ="head_pan_joint";
+ joint_cmds.position[0] = req_pan;
+ joint_cmds.name[1] ="head_tilt_joint";
+ joint_cmds.position[1] = req_tilt;
head_pub_.publish(joint_cmds);
fprintf(stdout,"teleop_head:: %f, %f\n", req_pan, req_tilt);
}
else if (!deadman_no_publish_)
{
- pr2_mechanism_msgs::JointState joint_cmd ;
- pr2_mechanism_msgs::JointStates joint_cmds;
-
- joint_cmd.name ="head_pan_joint";
- joint_cmd.position = req_pan;
- joint_cmds.joints.push_back(joint_cmd);
- joint_cmd.name="head_tilt_joint";
- joint_cmd.position = req_tilt;
- joint_cmds.joints.push_back(joint_cmd);
-
+ sensor_msgs::JointState joint_cmds;
+ joint_cmds.set_name_size(2);
+ joint_cmds.set_position_size(2);
+ joint_cmds.name[0] ="head_pan_joint";
+ joint_cmds.position[0] = req_pan;
+ joint_cmds.name[1] ="head_tilt_joint";
+ joint_cmds.position[1] = req_tilt;
head_pub_.publish(joint_cmds);
-
fprintf(stdout,"teleop_head:: %f, %f\n", req_pan, req_tilt);
}
}
Modified: pkg/trunk/pr2/teleop/teleop_head/teleop_head_keyboard.cpp
===================================================================
--- pkg/trunk/pr2/teleop/teleop_head/teleop_head_keyboard.cpp 2009-08-31 18:17:17 UTC (rev 23392)
+++ pkg/trunk/pr2/teleop/teleop_head/teleop_head_keyboard.cpp 2009-08-31 18:22:20 UTC (rev 23393)
@@ -37,8 +37,7 @@
#include <ros/ros.h>
-#include <pr2_mechanism_msgs/JointState.h>
-#include <pr2_mechanism_msgs/JointStates.h>
+#include <sensor_msgs/JointState.h>
#define HEAD_TOPIC "/head_controller/command"
@@ -67,7 +66,7 @@
req_tilt = 0.0;
req_pan = 0.0;
- head_pub_ = n_.advertise<pr2_mechanism_msgs::JointStates>(HEAD_TOPIC, 1);
+ head_pub_ = n_.advertise<sensor_msgs::JointState>(HEAD_TOPIC, 1);
n_.param("max_pan", max_pan, 2.7);
n_.param("max_tilt", max_tilt, 1.4);
@@ -176,16 +175,13 @@
if (dirty == true)
{
- pr2_mechanism_msgs::JointState joint_cmd ;
- pr2_mechanism_msgs::JointStates joint_cmds;
-
- joint_cmd.name ="head_pan_joint";
- joint_cmd.position = req_pan;
- joint_cmds.joints.push_back(joint_cmd);
- joint_cmd.name="head_tilt_joint";
- joint_cmd.position = req_tilt;
- joint_cmds.joints.push_back(joint_cmd);
-
+ sensor_msgs::JointState joint_cmds;
+ joint_cmds.set_name_size(2);
+ joint_cmds.set_position_size(2);
+ joint_cmds.name[0] ="head_pan_joint";
+ joint_cmds.position[0] = req_pan;
+ joint_cmds.name[1] ="head_tilt_joint";
+ joint_cmds.position[1] = req_tilt;
head_pub_.publish(joint_cmds) ;
Modified: pkg/trunk/pr2/teleop/teleop_pr2/manifest.xml
===================================================================
--- pkg/trunk/pr2/teleop/teleop_pr2/manifest.xml 2009-08-31 18:17:17 UTC (rev 23392)
+++ pkg/trunk/pr2/teleop/teleop_pr2/manifest.xml 2009-08-31 18:22:20 UTC (rev 23393)
@@ -9,7 +9,7 @@
<depend package="joy" />
<depend package="std_msgs" />
<depend package="geometry_msgs" />
- <depend package="pr2_mechanism_msgs" />
+ <depend package="sensor_msgs" />
<depend package="pr2_default_controllers" />
<depend package="roscpp" />
</package>
Modified: pkg/trunk/pr2/teleop/teleop_pr2/teleop_pr2.cpp
===================================================================
--- pkg/trunk/pr2/teleop/teleop_pr2/teleop_pr2.cpp 2009-08-31 18:17:17 UTC (rev 23392)
+++ pkg/trunk/pr2/teleop/teleop_pr2/teleop_pr2.cpp 2009-08-31 18:22:20 UTC (rev 23393)
@@ -37,8 +37,7 @@
#include "ros/ros.h"
#include "joy/Joy.h"
#include "geometry_msgs/Twist.h"
-#include "pr2_mechanism_msgs/JointState.h"
-#include "pr2_mechanism_msgs/JointStates.h"
+#include "sensor_msgs/JointState.h"
#include "std_msgs/Float64.h"
@@ -150,7 +149,7 @@
torso_pub_ = n_.advertise<std_msgs::Float64>(TORSO_TOPIC, 1);
if (head_button != 0)
- head_pub_ = n_.advertise<pr2_mechanism_msgs::JointStates>(HEAD_TOPIC, 1);
+ head_pub_ = n_.advertise<sensor_msgs::JointState>(HEAD_TOPIC, 1);
vel_pub_ = n_.advertise<geometry_msgs::Twist>("cmd_vel", 1);
@@ -246,16 +245,14 @@
// Head
if (head_button != 0)
{
- pr2_mechanism_msgs::JointState joint_cmd ;
- pr2_mechanism_msgs::JointStates joint_cmds;
+ sensor_msgs::JointState joint_cmds;
+ joint_cmds.set_name_size(2);
+ joint_cmds.set_position_size(2);
- joint_cmd.name ="head_pan_joint";
- joint_cmd.position = req_pan;
- joint_cmds.joints.push_back(joint_cmd);
- joint_cmd.name="head_tilt_joint";
- joint_cmd.position = req_tilt;
- joint_cmds.joints.push_back(joint_cmd);
-
+ joint_cmds.name[0] ="head_pan_joint";
+ joint_cmds.position[0] = req_pan;
+ joint_cmds.name[1] ="head_tilt_joint";
+ joint_cmds.position[1] = req_tilt;
head_pub_.publish(joint_cmds);
}
@@ -281,16 +278,13 @@
// Publish head
if (head_button != 0)
{
- pr2_mechanism_msgs::JointState joint_cmd ;
- pr2_mechanism_msgs::JointStates joint_cmds;
-
- joint_cmd.name ="head_pan_joint";
- joint_cmd.position = req_pan;
- joint_cmds.joints.push_back(joint_cmd);
- joint_cmd.name="head_tilt_joint";
- joint_cmd.position = req_tilt;
- joint_cmds.joints.push_back(joint_cmd);
-
+ sensor_msgs::JointState joint_cmds;
+ joint_cmds.set_name_size(2);
+ joint_cmds.set_position_size(2);
+ joint_cmds.name[0] ="head_pan_joint";
+ joint_cmds.position[0] = req_pan;
+ joint_cmds.name[1] ="head_tilt_joint";
+ joint_cmds.position[1] = req_tilt;
head_pub_.publish(joint_cmds);
}
Modified: pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/head_servoing_controller.h
===================================================================
--- pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/head_servoing_controller.h 2009-08-31 18:17:17 UTC (rev 23392)
+++ pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/head_servoing_controller.h 2009-08-31 18:22:20 UTC (rev 23393)
@@ -44,7 +44,7 @@
#include <tf/message_notifier.h>
#include <tf/transform_datatypes.h>
#include <robot_mechanism_controllers/joint_velocity_controller.h>
-#include <pr2_mechanism_msgs/JointStates.h>
+#include <sensor_msgs/JointState.h>
#include <geometry_msgs/PointStamped.h>
#include <boost/scoped_ptr.hpp>
#include <math.h>
@@ -80,7 +80,7 @@
ros::Subscriber sub_command_;
- void command(const pr2_mechanism_msgs::JointStatesConstPtr& command_msg);
+ void command(const sensor_msgs::JointStateConstPtr& command_msg);
void pointHead(const tf::MessageNotifier<geometry_msgs::PointStamped>::MessagePtr& point_msg);
void pointFrameOnHead(const tf::MessageNotifier<geometry_msgs::PointStamped>::MessagePtr& point_msg);
Modified: pkg/trunk/sandbox/experimental_controllers/manifest.xml
===================================================================
--- pkg/trunk/sandbox/experimental_controllers/manifest.xml 2009-08-31 18:17:17 UTC (rev 23392)
+++ pkg/trunk/sandbox/experimental_controllers/manifest.xml 2009-08-31 18:22:20 UTC (rev 23393)
@@ -10,7 +10,7 @@
<depend package="kdl_parser"/>
<depend package="rospy"/>
<depend package="pr2_controller_interface" />
- <depend package="pr2_mechanism_msgs" />
+ <depend package="sensor_msgs" />
<depend package="pr2_mechanism_model" />
<depend package="pr2_mechanism_control" />
<depend package="pluginlib" />
Modified: pkg/trunk/sandbox/experimental_controllers/src/head_servoing_controller.cpp
===================================================================
--- pkg/trunk/sandbox/experimental_controllers/src/head_servoing_controller.cpp 2009-08-31 18:17:17 UTC (rev 23392)
+++ pkg/trunk/sandbox/experimental_controllers/src/head_servoing_controller.cpp 2009-08-31 18:22:20 UTC (rev 23393)
@@ -100,7 +100,7 @@
// subscribe to head commands
- sub_command_ = node_.subscribe<pr2_mechanism_msgs::JointStates>("command", 1, &HeadServoingController::command, this);
+ sub_command_ = node_.subscribe<sensor_msgs::JointState>("command", 1, &HeadServoingController::command, this);
point_head_notifier_.reset(new MessageNotifier<geometry_msgs::PointStamped>(tf_,
boost::bind(&HeadServoingController::pointHead, this, _1),
@@ -135,21 +135,31 @@
head_tilt_controller_.update();
}
-void HeadServoingController::command(const pr2_mechanism_msgs::JointStatesConstPtr& command_msg)
+void HeadServoingController::command(const sensor_msgs::JointStateConstPtr& command_msg)
{
+ // do not use assert to check user input!
- assert(command_msg->joints.size() == 2);
- if(command_msg->joints[0].name == head_pan_controller_.joint_state_->joint_->name_)
+ if (command_msg->name.size() != 2 || command_msg->position.size() != 2){
+ ROS_ERROR("Head servoing controller expected joint command of size 2");
+ return;
+ }
+ if (command_msg->name[0] == head_pan_controller_.joint_state_->joint_->name_ &&
+ command_msg->name[1] == head_tilt_controller_.joint_state_->joint_->name_)
{
- pan_out_ = command_msg->joints[0].position;
- tilt_out_ = command_msg->joints[1].position;
+ pan_out_ = command_msg->position[0];
+ tilt_out_ = command_msg->position[1];
}
+ else if (command_msg->name[1] == head_pan_controller_.joint_state_->joint_->name_ &&
+ command_msg->name[0] == head_tilt_controller_.joint_state_->joint_->name_)
+
+ {
+ pan_out_ = command_msg->position[1];
+ tilt_out_ = command_msg->position[0];
+ }
else
{
- pan_out_ = command_msg->joints[1].position;
- tilt_out_ = command_msg->joints[0].position;
+ ROS_ERROR("Head servoing controller received invalid joint command");
}
-
}
Modified: pkg/trunk/sandbox/geometric_blur/.build_version
===================================================================
--- pkg/trunk/sandbox/geometric_blur/.build_version 2009-08-31 18:17:17 UTC (rev 23392)
+++ pkg/trunk/sandbox/geometric_blur/.build_version 2009-08-31 18:22:20 UTC (rev 23393)
@@ -1 +1 @@
-https://personalrobots.svn.sourceforge.net/svnroot/personalrobots/pkg/trunk/sandbox/geometric_blur:22743
+https://personalrobots.svn.sourceforge.net/svnroot/personalrobots/pkg/trunk/sandbox/geometric_blur:23383
Modified: pkg/trunk/sandbox/geometric_blur/.rosgcov_files
===================================================================
--- pkg/trunk/sandbox/geometric_blur/.rosgcov_files 2009-08-31 18:17:17 UTC (rev 23392)
+++ pkg/trunk/sandbox/geometric_blur/.rosgcov_files 2009-08-31 18:22:20 UTC (rev 23393)
@@ -1 +1 @@
-/u/msun/ros/ros-pkg/sandbox/geometric_blur src/gb.cpp
+/u/meeussen/ros/ros-pkg-svn/sandbox/geometric_blur src/gb.cpp
Modified: pkg/trunk/sandbox/hanoi/manifest.xml
===================================================================
--- pkg/trunk/sandbox/hanoi/manifest.xml 2009-08-31 18:17:17 UTC (rev 23392)
+++ pkg/trunk/sandbox/hanoi/manifest.xml 2009-08-31 18:22:20 UTC (rev 23393)
@@ -18,7 +18,6 @@
<depend package="move_arm_tools"/>
<depend package="tf"/>
<depend package="point_cloud_mapping"/>
- <depend package="pr2_mechanism_msgs"/>
<depend package="bullet"/>
<depend package="move_arm_tools"/>
Modified: pkg/trunk/sandbox/hanoi/src/Hanoi.cpp
===================================================================
--- pkg/trunk/sandbox/hanoi/src/Hanoi.cpp 2009-08-31 18:17:17 UTC (rev 23392)
+++ pkg/trunk/sandbox/hanoi/src/Hanoi.cpp 2009-08-31 18:22:20 UTC (rev 23393)
@@ -1,5 +1,4 @@
-#include <pr2_mechanism_msgs/JointStates.h>
-#include <pr2_mechanism_msgs/JointState.h>
+#include <sensor_msgs/JointState.h>
#include <point_cloud_mapping/geometry/angles.h>
#include <point_cloud_mapping/geometry/point.h>
@@ -26,7 +25,7 @@
// Subscribe to the blobs topic
blobSubscriber_ = nodeHandle_.subscribe("blobs", 100, &Hanoi::BlobCB, this);
- headPublisher_ = nodeHandle_.advertise<pr2_mechanism_msgs::JointStates>("pan_tilt", 1, true);
+ headPublisher_ = nodeHandle_.advertise<sensor_msgs::JointState>("pan_tilt", 1, true);
// Create the publisher for cylinder data
cylinderPublisher_ = nodeHandle_.advertise<hanoi::Cylinders>("cylinders", 1);
@@ -57,19 +56,17 @@
// Pan and tilt the head
void Hanoi::CommandHead(float pan, float tilt)
{
- pr2_mechanism_msgs::JointStates jointStatesMsg;
- pr2_mechanism_msgs::JointState panJoint, tiltJoint;
+ sensor_msgs::JointState jointStateMsg;
+ jointStateMsg.set_name_size(2);
+ jointStateMsg.set_position_size(2);
- panJoint.name = "head_pan_joint";
- panJoint.position = pan;
+ jointStateMsg.name[0] = "head_pan_joint";
+ jointStateMsg.position[0] = pan;
- tiltJoint.name = "head_tilt_joint";
- tiltJoint.position = tilt;
+ jointStateMsg.name[1] = "head_tilt_joint";
+ jointStateMsg.position[1] = tilt;
- jointStatesMsg.joints.push_back(panJoint);
- jointStatesMsg.joints.push_back(tiltJoint);
-
- headPublisher_.publish( jointStatesMsg );
+ headPublisher_.publish( jointStateMsg );
}
////////////////////////////////////////////////////////////////////////////////
Modified: pkg/trunk/sandbox/move_arm_tools/manifest.xml
===================================================================
--- pkg/trunk/sandbox/move_arm_tools/manifest.xml 2009-08-31 18:17:17 UTC (rev 23392)
+++ pkg/trunk/sandbox/move_arm_tools/manifest.xml 2009-08-31 18:22:20 UTC (rev 23393)
@@ -11,7 +11,7 @@
<depend package="manipulation_srvs" />
<depend package="motion_planning_msgs" />
<depend package="planning_environment" />
- <depend package="pr2_mechanism_msgs" />
+ <depend package="sensor_msgs" />
<depend package="tf" />
<depend package="pr2_ik" />
<depend package="kdl"/>
Modified: pkg/trunk/sandbox/robot_voxelizer/include/robot_voxelizer/robot_voxelizer.h
===================================================================
--- pkg/trunk/sandbox/robot_voxelizer/include/robot_voxelizer/robot_voxelizer.h 2009-08-31 18:17:17 UTC (rev 23392)
+++ pkg/trunk/sandbox/robot_voxelizer/include/robot_voxelizer/robot_voxelizer.h 2009-08-31 18:22:20 UTC (rev 23393)
@@ -40,7 +40,7 @@
#include <planning_environment/models/robot_models.h>
#include <planning_environment/monitors/kinematic_model_state_monitor.h>
#include <planning_models/kinematic.h>
-#include <pr2_mechanism_msgs/MechanismState.h>
+#include <sensor_msgs/JointState.h>
#include "visualization_msgs/Marker.h"
class RobotVoxelizer
@@ -113,7 +113,7 @@
std::vector< std::pair < std::vector<bodies::Body *>, std::vector<bodies::Body *> > > scg_bodies_;
/** \brief Callback function for mechanismState that updates the pose of the bodies. */
- void jointStatesCallback(const pr2_mechanism_msgs::JointStatesConstPtr &joint_states);
+ void jointStatesCallback(const sensor_msgs::JointStateConstPtr &joint_states);
/** \brief Convert from world coordinates to voxel grid coordinates. */
void worldToGrid(btVector3 origin, double wx, double wy, double wz, int &gx, int &gy, int &gz) const {
Modified: pkg/trunk/sandbox/robot_voxelizer/manifest.xml
===================================================================
--- pkg/trunk/sandbox/robot_voxelizer/manifest.xml 2009-08-31 18:17:17 UTC (rev 23392)
+++ pkg/trunk/sandbox/robot_voxelizer/manifest.xml 2009-08-31 18:22:20 UTC (rev 23393)
@@ -4,12 +4,12 @@
<author>Benjamin Cohen /bc...@wi...</author>
<license>BSD</license>
<depend package="roscpp"/>
- <depend package="geometric_shapes"/>
+ <depend package="geometric_shapes"/>
<depend package="planning_environment" />
- <depend package="bullet" />
+ <depend package="bullet" />
<depend package="planning_models" />
- <depend package="visualization_msgs" />
- <depend package="pr2_mechanism_msgs" />
+ <depend package="visualization_msgs" />
+ <depend package="sensor_msgs" />
<url></url>
<export>
Modified: pkg/trunk/sandbox/robot_voxelizer/src/robot_voxelizer.cpp
===================================================================
--- pkg/trunk/sandbox/robot_voxelizer/src/robot_voxelizer.cpp 2009-08-31 18:17:17 UTC (rev 23392)
+++ pkg/trunk/sandbox/robot_voxelizer/src/robot_voxelizer.cpp 2009-08-31 18:22:20 UTC (rev 23393)
@@ -201,7 +201,7 @@
monitor_->getKinematicModel()->unlock();
}
-void RobotVoxelizer::jointStatesCallback(const pr2_mechanism_msgs::JointStatesConstPtr &joint_states)
+void RobotVoxelizer::jointStatesCallback(const sensor_msgs::JointStateConstPtr &joint_states)
{
// updateSelfCollisionBodies();
Modified: pkg/trunk/sandbox/teleop_anti_collision/manifest.xml
===================================================================
--- pkg/trunk/sandbox/teleop_anti_collision/manifest.xml 2009-08-31 18:17:17 UTC (rev 23392)
+++ pkg/trunk/sandbox/teleop_anti_collision/manifest.xml 2009-08-31 18:22:20 UTC (rev 23393)
@@ -21,7 +21,7 @@
<depend package="actionlib" />
<depend package="move_base_msgs" />
- <depend package="pr2_mechanism_msgs" />
+ <depend package="sensor_msgs" />
<depend package="std_msgs" /><depend package="joy"></depend>
<export>
<cpp cflags="-I${prefix}/include -I${prefix}/msg/cpp"/>
Modified: pkg/trunk/sandbox/teleop_anti_collision/src/teleop_goal_projection.cpp
===================================================================
--- pkg/trunk/sandbox/teleop_anti_collision/src/teleop_goal_projection.cpp 2009-08-31 18:17:17 UTC (rev 23392)
+++ pkg/trunk/sandbox/teleop_anti_collision/src/teleop_goal_projection.cpp 2009-08-31 18:22:20 UTC (rev 23393)
@@ -45,8 +45,7 @@
#include <ros/time.h>
#include "tf/transform_listener.h"
-#include "pr2_mechanism_msgs/JointState.h"
-#include "pr2_mechanism_msgs/JointStates.h"
+#include <sensor_msgs/JointState.h>
#define TORSO_TOPIC "/torso_lift_controller/set_command"
#define HEAD_TOPIC "/head_controller/command"
@@ -157,7 +156,7 @@
if(torso_dn_button != 0)
torso_pub_ = node_handle_.advertise<std_msgs::Float64> (TORSO_TOPIC, 1);
if(head_button != 0)
- head_pub_ = node_handle_.advertise<pr2_mechanism_msgs::JointStates> (HEAD_TOPIC, 1);
+ head_pub_ = node_handle_.advertise<sensor_msgs::JointState> (HEAD_TOPIC, 1);
goal_pub_ = node_handle_.advertise<geometry_msgs::PoseStamped> ("/goal", 1);
joy_sub_ = node_handle_.subscribe("joy",1,&TeleopGoalProjection::joy_cb,this);
ROS_DEBUG("done with ctor\n");
@@ -285,15 +284,13 @@
// Head
if(head_button != 0)
{
- pr2_mechanism_msgs::JointState joint_cmd ;
- pr2_mechanism_msgs::JointStates joint_cmds;
-
- joint_cmd.name ="head_pan_joint";
- joint_cmd.position = req_pan;
- joint_cmds.joints.push_back(joint_cmd);
- joint_cmd.name="head_tilt_joint";
- joint_cmd.position = req_tilt;
- joint_cmds.joints.push_back(joint_cmd);
+ sensor_msgs::JointState joint_cmds;
+ joint_cmds.set_name_size(2);
+ joint_cmds.set_position_size(2);
+ joint_cmds.name[0] ="head_pan_joint";
+ joint_cmds.position[0] = req_pan;
+ joint_cmds.name[1]="head_tilt_joint";
+ joint_cmds.position[1] = req_tilt;
head_pub_.publish(joint_cmds);
}
@@ -347,15 +344,13 @@
// Publish head
if(head_button != 0)
{
- pr2_mechanism_msgs::JointState joint_cmd ;
- pr2_mechanism_msgs::JointStates joint_cmds;
-
- joint_cmd.name ="head_pan_joint";
- joint_cmd.position = req_pan;
- joint_cmds.joints.push_back(joint_cmd);
- joint_cmd.name="head_tilt_joint";
- joint_cmd.position = req_tilt;
- joint_cmds.joints.push_back(joint_cmd);
+ sensor_msgs::JointState joint_cmds;
+ joint_cmds.set_name_size(2);
+ joint_cmds.set_position_size(2);
+ joint_cmds.name[0] ="head_pan_joint";
+ joint_cmds.position[0] = req_pan;
+ joint_cmds.name[1]="head_tilt_joint";
+ joint_cmds.position[1] = req_tilt;
head_pub_.publish(joint_cmds);
}
Modified: pkg/trunk/stacks/calibration/joint_states_settler/include/joint_states_settler/deflated_joint_states.h
===================================================================
--- pkg/trunk/stacks/calibration/joint_states_settler/include/joint_states_settler/deflated_joint_states.h 2009-08-31 18:17:17 UTC (rev 23392)
+++ pkg/trunk/stacks/calibration/joint_states_settler/include/joint_states_settler/deflated_joint_states.h 2009-08-31 18:22:20 UTC (rev 23393)
@@ -35,7 +35,7 @@
#ifndef JOINT_STATES_SETTLER_DEFLATED_JOINT_STATES_H_
#define JOINT_STATES_SETTLER_DEFLATED_JOINT_STATES_H_
-#include <pr2_mechanism_msgs/JointStates.h>
+#include <sensor_msgs/JointState.h>
#include <settlerlib/deflated.h>
namespace joint_states_settler
@@ -44,7 +44,7 @@
class DeflatedJointStates : public settlerlib::Deflated
{
public:
- pr2_mechanism_msgs::JointStatesConstPtr msg_;
+ sensor_msgs::JointStateConstPtr msg_;
};
}
Modified: pkg/trunk/stacks/calibration/joint_states_settler/include/joint_states_settler/joint_states_deflater.h
===================================================================
--- pkg/trunk/stacks/calibration/joint_states_settler/include/joint_states_settler/joint_states_deflater.h 2009-08-31 18:17:17 UTC (rev 23392)
+++ pkg/trunk/stacks/calibration/joint_states_settler/include/joint_states_settler/joint_states_deflater.h 2009-08-31 18:22:20 UTC (rev 23393)
@@ -35,7 +35,7 @@
#ifndef JOINT_STATES_SETTLER_JOINT_STATES_DEFLATER_H_
#define JOINT_STATES_SETTLER_JOINT_STATES_DEFLATER_H_
-#include <pr2_mechanism_msgs/JointStates.h>
+#include <sensor_msgs/JointState.h>
#include <settlerlib/deflated.h>
#include "deflated_joint_states.h"
@@ -66,7 +66,7 @@
* \param joint_states Incoming JointStates message
* \param Ouput datatype. Stores the deflated data, along with the original joint states message
*/
- void deflate(const pr2_mechanism_msgs::JointStatesConstPtr& joint_states, DeflatedJointStates& deflated_elem);
+ void deflate(const sensor_msgs::JointStateConstPtr& joint_states, DeflatedJointStates& deflated_elem);
private:
std::vector<unsigned int> mapping_;
@@ -76,7 +76,7 @@
* \brief Given a stereotypical JointStates message, computes the mapping
* from JointStates to the deflated data
*/
- void updateMapping(const pr2_mechanism_msgs::JointStates& joint_states);
+ void updateMapping(const sensor_msgs::JointState& joint_states);
};
Modified: pkg/trunk/stacks/calibration/joint_states_settler/include/joint_states_settler/joint_states_settler.h
===================================================================
--- pkg/trunk/stacks/calibration/joint_states_settler/include/joint_states_settler/joint_states_settler.h 2009-08-31 18:17:17 UTC (rev 23392)
+++ pkg/trunk/stacks/calibration/joint_states_settler/include/joint_states_settler/joint_states_settler.h 2009-08-31 18:22:20 UTC (rev 23393)
@@ -40,7 +40,7 @@
#include <boost/shared_ptr.hpp>
#include <calibration_msgs/Interval.h>
-#include <pr2_mechanism_msgs/JointStates.h>
+#include <sensor_msgs/JointState.h>
#include <settlerlib/sorted_deque.h>
@@ -59,7 +59,7 @@
bool configure(const joint_states_settler::ConfigGoal& goal);
- calibration_msgs::Interval add(const pr2_mechanism_msgs::JointStatesConstPtr msg);
+ calibration_msgs::Interval add(const sensor_msgs::JointStateConstPtr msg);
private:
bool configured_;
Modified: pkg/trunk/stacks/calibration/joint_states_settler/manifest.xml
===================================================================
--- pkg/trunk/stacks/calibration/joint_states_settler/manifest.xml 2009-08-31 18:17:17 UTC (rev 23392)
+++ pkg/trunk/stacks/calibration/joint_states_settler/manifest.xml 2009-08-31 18:22:20 UTC (rev 23393)
@@ -14,13 +14,11 @@
<!-- common_msgs -->
<depend package="actionlib_msgs"/>
+ <depend package="sensor_msgs"/>
<!-- common -->
<depend package="actionlib"/>
- <!-- mechanism -->
- <depend package="pr2_mechanism_msgs"/>
-
<!-- calibration -->
<depend package="settlerlib"/>
<depend package="calibration_msgs"/>
Modified: pkg/trunk/stacks/calibration/joint_states_settler/src/joint_states_deflater.cpp
===================================================================
--- pkg/trunk/stacks/calibration/joint_states_settler/src/joint_states_deflater.cpp 2009-08-31 18:17:17 UTC (rev 23392)
+++ pkg/trunk/stacks/calibration/joint_states_settler/src/joint_states_deflater.cpp 2009-08-31 18:22:20 UTC (rev 23393)
@@ -48,30 +48,35 @@
mapping_.resize(joint_names_.size());
}
-void JointStatesDeflater::deflate(const pr2_mechanism_msgs::JointStatesConstPtr& joint_states, DeflatedJointStates& deflated_elem)
+void JointStatesDeflater::deflate(const sensor_msgs::JointStateConstPtr& joint_states, DeflatedJointStates& deflated_elem)
{
+ if (joint_states->get_name_size() != joint_states->get_position_size()){
+ ROS_ERROR("JointStatesDeflater got invalid joint state message");
+ return;
+ }
+
if (mapping_.size() != joint_names_.size())
updateMapping(*joint_states);
-
+
const unsigned int N = joint_names_.size();
-
+
deflated_elem.channels_.resize(N);
-
+
for (unsigned int i=0; i<N; i++)
{
- if ( mapping_[i] >= joint_states->joints.size() )
+ if ( mapping_[i] >= joint_states->name.size() )
updateMapping(*joint_states);
-
- if ( joint_states->joints[mapping_[i]].name != joint_names_[i])
+
+ if ( joint_states->name[mapping_[i]] != joint_names_[i])
updateMapping(*joint_states);
-
+
deflated_elem.header = joint_states->header;
- deflated_elem.channels_[i] = joint_states->joints[mapping_[i]].position;
+ deflated_elem.channels_[i] = joint_states->position[mapping_[i]];
deflated_elem.msg_ = joint_states;
}
}
-void JointStatesDeflater::updateMapping(const pr2_mechanism_msgs::JointStates& joint_states)
+void JointStatesDeflater::updateMapping(const sensor_msgs::JointState& joint_states)
{
ROS_DEBUG("Updating the JointStates mapping");
@@ -82,9 +87,9 @@
for (unsigned int i=0; i<N; i++)
{
bool mapping_found = false;
- for (unsigned int j=0; j<joint_states.joints.size(); j++)
+ for (unsigned int j=0; j<joint_states.name.size(); j++)
{
- if ( joint_names_[i] == joint_states.joints[j].name)
+ if ( joint_names_[i] == joint_states.name[j])
{
mapping_[i] = j;
mapping_found = true;
Modified: pkg/trunk/stacks/calibration/joint_states_settler/src/joint_states_settler.cpp
===================================================================
--- pkg/trunk/stacks/calibration/joint_states_settler/src/joint_states_settler.cpp 2009-08-31 18:17:17 UTC (rev 23392)
+++ pkg/trunk/stacks/calibration/joint_states_settler/src/joint_states_settler.cpp 2009-08-31 18:22:20 UTC (rev 23393)
@@ -73,7 +73,7 @@
return true;
}
-calibration_msgs::Interval JointStatesSettler::add(const pr2_mechanism_msgs::JointStatesConstPtr msg)
+calibration_msgs::Interval JointStatesSettler::add(const sensor_msgs::JointStateConstPtr msg)
{
if (!configured_)
{
Modified: pkg/trunk/stacks/calibration/joint_states_settler/src/joint_states_settler_node.cpp
===================================================================
--- pkg/trunk/stacks/calibration/joint_states_settler/src/joint_states_settler_node.cpp 2009-08-31 18:17:17 UTC (rev 23392)
+++ pkg/trunk/stacks/calibration/joint_states_settler/src/joint_states_settler_node.cpp 2009-08-31 18:22:20 UTC (rev 23393)
@@ -94,7 +94,7 @@
return config;
}
-void jointStatesCallback(ros::Publisher* pub, JointStatesSettler* settler, const pr2_mechanism_msgs::JointStatesConstPtr& msg)
+void jointStatesCallback(ros::Publisher* pub, JointStatesSettler* settler, const sensor_msgs::JointStateConstPtr& msg)
{
pub->publish(settler->add(msg));
}
@@ -115,7 +115,7 @@
// Input
- boost::function<void (const pr2_mechanism_msgs::JointStatesConstPtr&)> cb = boost::bind(&jointStatesCallback, &pub, &settler, _1);
+ boost::function<void (const sensor_msgs::JointStateConstPtr&)> cb = boost::bind(&jointStatesCallback, &pub, &settler, _1);
ros::Subscriber sub = n.subscribe(std::string("joint_states"), 1, cb);
ros::spin();
Added: pkg/trunk/stacks/common_msgs/sensor_msgs/msg/JointState.msg
===================================================================
--- pkg/trunk/stacks/common_msgs/sensor_msgs/msg/JointState.msg (rev 0)
+++ pkg/trunk/stacks/common_msgs/sensor_msgs/msg/JointState.msg 2009-08-31 18:22:20 UTC (rev 23393)
@@ -0,0 +1,6 @@
+Header header
+
+string[] name
+float64[] position
+float64[] velocity
+float64[] effort
Modified: pkg/trunk/stacks/pr2_common/pr2_msgs/manifest.xml
===================================================================
--- pkg/trunk/stacks/pr2_common/pr2_msgs/manifest.xml 2009-08-31 18:17:17 UTC (rev 23392)
+++ pkg/trunk/stacks/pr2_common/pr2_msgs/manifest.xml 2009-08-31 18:22:20 UTC (rev 23393)
@@ -12,7 +12,6 @@
<depend package="std_msgs"/>
<depend package="geometry_msgs"/>
- <depend package="pr2_mechanism_msgs" />
<depend package="rostest"/>
<depend package="rosbagmigration"/>
Modified: pkg/trunk/stacks/pr2_mechanism/pr2_mechanism_control/include/pr2_mechanism_control/mechanism_control.h
===================================================================
--- pkg/trunk/stacks/pr2_mechanism/pr2_mechanism_control/include/pr2_mechanism_control/mechanism_control.h 2009-08-31 18:17:17 UTC (rev 23392)
+++ pkg/trunk/stacks/pr2_mechanism/pr2_mechanism_control/include/pr2_mechanism_control/mechanism_control.h 2009-08-31 18:22:20 UTC (rev 23393)
@@ -51,7 +51,7 @@
#include <pr2_mechanism_msgs/KillController.h>
#include <pr2_mechanism_msgs/SwitchController.h>
#include <pr2_mechanism_msgs/MechanismState.h>
-#include <pr2_mechanism_msgs/JointStates.h>
+#include <sensor_msgs/JointState.h>
#include <diagnostic_msgs/DiagnosticArray.h>
@@ -108,7 +108,7 @@
void publishDiagnostics();
void publishState();
realtime_tools::RealtimePublisher<diagnostic_msgs::DiagnosticArray> pub_diagnostics_;
- realtime_tools::RealtimePublisher<pr2_mechanism_msgs::JointStates> pub_joints_;
+ realtime_tools::RealtimePublisher<sensor_msgs::JointState> pub_joint_state_;
realtime_tools::RealtimePublisher<pr2_mechanism_msgs::MechanismState> pub_mech_state_;
double publish_period_state_, last_published_state_;
double publish_period_diagnostics_, last_published_diagnostics_;
Modified: pkg/trunk/stacks/pr2_mechanism/pr2_mechanism_control/src/mechanism_control.cpp
===================================================================
--- pkg/trunk/stacks/pr2_mechanism/pr2_mechanism_control/src/mechanism_control.cpp 2009-08-31 18:17:17 UTC (rev 23392)
+++ pkg/trunk/stacks/pr2_mechanism/pr2_mechanism_control/src/mechanism_control.cpp 2009-08-31 18:22:20 UTC (rev 23393)
@@ -53,7 +53,7 @@
current_controllers_list_(0),
used_by_realtime_(-1),
pub_diagnostics_(node_, "/diagnostics", 1),
- pub_joints_(node_, "joint_states", 1),
+ pub_joint_state_(node_, "joint_states", 1),
pub_mech_state_(node_, "mechanism_state", 1),
last_published_state_(ros::Time::now().toSec()),
last_published_diagnostics_(ros::Time::now().toSec())
@@ -80,8 +80,11 @@
if (type == urdf::Joint::REVOLUTE || type == urdf::Joint::CONTINUOUS || type == urdf::Joint::PRISMATIC)
++joints_size;
}
- pub_joints_.msg_.set_joints_size(joints_size);
pub_mech_state_.msg_.set_joint_states_size(joints_size);
+ pub_joint_state_.msg_.set_name_size(joints_size);
+ pub_joint_state_.msg_.set_position_size(joints_size);
+ pub_joint_state_.msg_.set_velocity_size(joints_size);
+ pub_joint_state_.msg_.set_effort_size(joints_size);
// Advertise services
srv_list_controllers_ = node_.advertiseService("list_controllers", &MechanismControl::listControllersSrv, this);
@@ -513,14 +516,15 @@
if (type == urdf::Joint::REVOLUTE || type == urdf::Joint::CONTINUOUS || type == urdf::Joint::PRISMATIC)
{
assert(j < pub_mech_state_.msg_.get_joint_states_size());
- pr2_mechanism_msgs::JointState *out = &pub_mech_state_.msg_.joint_states[j++];
mechanism::JointState *in = &state_->joint_states_[i];
+ pr2_mechanism_msgs::JointState *out = &pub_mech_state_.msg_.joint_states[j];
out->name = model_.joints_[i]->name_;
out->position = in->position_;
out->velocity = in->velocity_;
out->applied_effort = in->applied_effort_;
out->commanded_effort = in->commanded_effort_;
out->is_calibrated = in->calibrated_;
+ j++;
}
}
@@ -557,7 +561,7 @@
pub_mech_state_.unlockAndPublish();
}
- if (pub_joints_.trylock())
+ if (pub_joint_state_.trylock())
{
unsigned int j = 0;
for (unsigned int i = 0; i < model_.joints_.size(); ++i)
@@ -565,21 +569,23 @@
int type = state_->joint_states_[i].joint_->type_;
if (type == urdf::Joint::REVOLUTE || type == urdf::Joint::CONTINUOUS || type == urdf::Joint::PRISMATIC)
{
- assert(j < pub_joints_.msg_.get_joints_size());
- pr2_mechanism_msgs::JointState *out = &pub_joints_.msg_.joints[j++];
+ assert(j < pub_joint_state_.msg_.get_name_size());
+ assert(j < pub_joint_state_.msg_.get_position_size());
+ assert(j < pub_joint_state_.msg_.get_velocity_size());
+ assert(j < pub_joint_state_.msg_.get_effort_size());
mechanism::JointState *in = &state_->joint_states_[i];
- out->name = model_.joints_[i]->name_;
- out->position = in->position_;
- out->velocity = in->velocity_;
- out->applied_effort = in->applied_effort_;
- out->commanded_effort = in->commanded_effort_;
- out->is_calibrated = in->calibrated_;
+ pub_joint_state_.msg_.name[j] = model_.joints_[i]->name_;
+ pub_joint_state_.msg_.position[j] = in->position_;
+ pub_joint_state_.msg_.velocity[j] = in->velocity_;
+ pub_joint_state_.msg_.effort[j] = in->applied_effort_;
+
+ j++;
}
}
- pub_joints_.msg_.header.stamp = ros::Time::now();
+ pub_joint_state_.msg_.header.stamp = ros::Time::now();
- pub_joints_.unlockAndPublish();
+ pub_joint_state_.unlockAndPublish();
}
}
}
Deleted: pkg/trunk/stacks/pr2_mechanism/pr2_mechanism_msgs/msg/JointStates.msg
===================================================================
--- pkg/trunk/stacks/pr2_mechanism/pr2_mechanism_msgs/msg/JointStates.msg 2009-08-31 18:17:17 UTC (rev 23392)
+++ pkg/trunk/stacks/pr2_mechanism/pr2_mechanism_msgs/msg/JointStates.msg 2009-08-31 18:22:20 UTC (rev 23393)
@@ -1,2 +0,0 @@
-Header header
-pr2_mechanism_msgs/JointState[] joints
Modified: pkg/trunk/stacks/pr2_simulator/gazebo_plugin/manifest.xml
===================================================================
--- pkg/trunk/stacks/pr2_simulator/gazebo_plugin/manifest.xml 2009-08-31 18:17:17 UTC (rev 23392)
+++ pkg/trunk/stacks/pr2_simulator/gazebo_plugin/manifest.xml 2009-08-31 18:22:20 UTC (rev 23393)
@@ -14,7 +14,6 @@
<depend package="rospy"/>
<depend package="std_msgs" />
<depend package="nav_msgs" />
- <depend package="pr2_mechanism_msgs" />
<depend package="sensor_msgs" />
<depend package="opencv_latest" />
<depend package="sensor_msgs" />
Modified: pkg/trunk/stacks/robot_model/robot_state_publisher/include/robot_state_publisher/joint_state_listener.h
===================================================================
--- pkg/trunk/stacks/robot_model/robot_state_publisher/include/robot_state_publisher/joint_state_listener.h 2009-08-31 18:17:17 UTC (rev 23392)
+++ pkg/trunk/stacks/robot_model/robot_state_publisher/include/robot_state_publisher/joint_state_listener.h 2009-08-31 18:22:20 UTC (rev 23393)
@@ -39,14 +39,14 @@
#include <kdl/tree.hpp>
#include <ros/ros.h>
-#include <pr2_mechanism_msgs/JointStates.h>
+#include <sensor_msgs/JointState.h>
#include "robot_state_publisher/robot_state_publisher.h"
using namespace std;
using namespace ros;
using namespace KDL;
-typedef boost::shared_ptr<pr2_mechanism_msgs::JointStates const> JointStateConstPtr;
+typedef boost::shared_ptr<sensor_msgs::JointState const> JointStateConstPtr;
namespace robot_state_publisher{
Modified: pkg/trunk/stacks/robot_model/robot_state_publisher/manifest.xml
===================================================================
--- pkg/trunk/stacks/robot_model/robot_state_publisher/manifest.xml 2009-08-31 18:17:17 UTC (rev 23392)
+++ pkg/trunk/stacks/robot_model/robot_state_publisher/manifest.xml 2009-08-31 18:22:20 UTC (rev 23393)
@@ -14,7 +14,7 @@
<url>http://pr.willowgarage.com/wiki/robot_state_publisher</url>
<depend package="kdl_parser" />
- <depend package="pr2_mechanism_msgs" />
+ <depend package="sensor_msgs" />
<depend package="roscpp" />
<depend package="tf" />
<depend package="tf_conversions" />
Modified: pkg/trunk/stacks/robot_model/robot_state_publisher/src/joint_state_listener.cpp
===================================================================
--- pkg/trunk/stacks/robot_model/robot_state_publisher/src/joint_state_listener.cpp 2009-08-31 18:17:17 UTC (rev 23392)
+++ pkg/trunk/stacks/robot_model/robot_state_publisher/src/joint_state_listener.cpp 2009-08-31 18:22:20 UTC (rev 23393)
@@ -36,7 +36,6 @@
#include <kdl/tree.hpp>
#include <ros/ros.h>
-#include <pr2_mechanism_msgs/JointStates.h>
#include "robot_state_publisher/robot_state_publisher.h"
#include "robot_state_publisher/joint_state_listener.h"
@@ -67,10 +66,20 @@
void JointStateListener::callbackJointState(const JointStateConstPtr& state)
{
+ if (state->get_name_size() == 0){
+ ROS_ERROR("Robot state publisher received an empty joint state vector");
+ return;
+ }
+
+ if (state->get_name_size() != state->get_position_size()){
+ ROS_ERROR("Robot state publisher received an invalid joint state vector");
+ return;
+ }
+
// get joint positions from state message
map<string, double> joint_positions;
- for (unsigned int i=0; i<state->joints.size(); i++)
- joint_positions.insert(make_pair(state->joints[i].name, state->joints[i].position));
+ for (unsigned int i=0; i<state->name.size(); i++)
+ joint_positions.insert(make_pair(state->name[i], state->position[i]));
state_publisher_.publishTransforms(joint_positions, state->header.stamp);
publish_rate_.sleep();
}
Modified: pkg/trunk/stacks/robot_model/robot_state_publisher/src/state_publisher.cpp
===================================================================
--- pkg/trunk/stacks/robot_model/robot_state_publisher/src/state_publisher.cpp 2009-08-31 18:17:17 UTC (rev 23392)
+++ pkg/trunk/stacks/robot_model/robot_state_publisher/src/state_publisher.cpp 2009-08-31 18:22:20 UTC (rev 23393)
@@ -38,7...
[truncated message content] |
