00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00037 #ifndef UNNORMALIZE_JOINT_TRAJECTORY_H_
00038 #define UNNORMALIZE_JOINT_TRAJECTORY_H_
00039
00040 #include <ros/ros.h>
00041 #include <urdf/model.h>
00042 #include <sensor_msgs/JointState.h>
00043 #include <spline_smoother/spline_smoother.h>
00044 #include <spline_smoother/spline_smoother_utils.h>
00045
00046 namespace joint_normalization_filters
00047 {
00048
00052 template <typename T>
00053 class UnNormalizeJointTrajectory: public spline_smoother::SplineSmoother<T>
00054 {
00055 private:
00057 ros::NodeHandle nh_;
00058
00060 urdf::Model robot_model_;
00061
00063 bool robot_model_initialized_;
00064
00066 sensor_msgs::JointState::ConstPtr raw_joint_states_;
00067
00069 ros::Subscriber joint_states_sub_;
00070
00072 void jointStatesCallback(const sensor_msgs::JointState::ConstPtr& states);
00073
00074 public:
00075 UnNormalizeJointTrajectory();
00076 virtual ~UnNormalizeJointTrajectory();
00077
00078 virtual bool smooth(const T& trajectory_in,
00079 T& trajectory_out) const;
00080 };
00081
00082 template <typename T>
00083 UnNormalizeJointTrajectory<T>::UnNormalizeJointTrajectory()
00084 {
00085 joint_states_sub_ = nh_.subscribe("joint_states", 1, &UnNormalizeJointTrajectory::jointStatesCallback, this);
00086
00087 std::string urdf_xml,full_urdf_xml;
00088 nh_.param("urdf_xml", urdf_xml, std::string("robot_description"));
00089 if(!nh_.getParam(urdf_xml,full_urdf_xml))
00090 {
00091 ROS_ERROR("Could not load the xml from parameter server: %s\n", urdf_xml.c_str());
00092 robot_model_initialized_ = false;
00093 }
00094 else
00095 {
00096 robot_model_.initString(full_urdf_xml);
00097 robot_model_initialized_ = true;
00098 }
00099 }
00100
00101 template <typename T>
00102 UnNormalizeJointTrajectory<T>::~UnNormalizeJointTrajectory()
00103 {
00104 }
00105
00106 template <typename T>
00107 bool UnNormalizeJointTrajectory<T>::smooth(const T& trajectory_in,
00108 T& trajectory_out) const
00109 {
00110 trajectory_out = trajectory_in;
00111
00112 if (!spline_smoother::checkTrajectoryConsistency(trajectory_out))
00113 return false;
00114
00115 if (!robot_model_initialized_)
00116 {
00117 ROS_ERROR("Robot model not initialized; can not tell continuous joints");
00118 return false;
00119 }
00120
00121 if (raw_joint_states_.get() == NULL)
00122 {
00123 ROS_ERROR("Did not receive current robot state message");
00124 return false;
00125 }
00126
00127 std::vector<double> current_values;
00128 std::vector<int> wraparound;
00129 trajectory_msgs::JointTrajectory input_trajectory = trajectory_in.trajectory;
00130 for (size_t i=0; i<input_trajectory.joint_names.size(); i++)
00131 {
00132 std::string name = input_trajectory.joint_names[i];
00133 size_t j;
00134 for (j=0; j<raw_joint_states_->name.size(); j++)
00135 {
00136 if (name == raw_joint_states_->name[j]) break;
00137 }
00138 if ( j==raw_joint_states_->name.size())
00139 {
00140 ROS_ERROR("Joint name %s not found in raw joint state message", name.c_str());
00141 return false;
00142 }
00143
00144 current_values.push_back(raw_joint_states_->position.at(j));
00145
00146 boost::shared_ptr<const urdf::Joint> joint = robot_model_.getJoint(name);
00147 if (joint.get() == NULL)
00148 {
00149 ROS_ERROR("Joint name %s not found in urdf model", name.c_str());
00150 return false;
00151 }
00152 if (joint->type == urdf::Joint::CONTINUOUS) wraparound.push_back(1);
00153 else wraparound.push_back(0);
00154 }
00155
00156 trajectory_msgs::JointTrajectory normalized_trajectory = input_trajectory;
00157 for (size_t i=0; i<normalized_trajectory.points.size(); i++)
00158 {
00159 for (size_t j=0; j<normalized_trajectory.points[i].positions.size(); j++ )
00160 {
00161 if (!wraparound.at(j)) continue;
00162 double current = current_values.at(j);
00163 double traj = normalized_trajectory.points[i].positions[j];
00164 while ( current - traj > M_PI ) traj += 2*M_PI;
00165 while ( traj - current > M_PI ) traj -= 2*M_PI;
00166 ROS_DEBUG("Normalizing joint %s from %f to %f", normalized_trajectory.joint_names.at(j).c_str(),
00167 normalized_trajectory.points[i].positions[j], traj);
00168 normalized_trajectory.points[i].positions[j] = traj;
00169
00170 current_values.at(j) = traj;
00171 }
00172 }
00173 trajectory_out.trajectory = normalized_trajectory;
00174 return true;
00175 }
00176
00178 template <typename T>
00179 void UnNormalizeJointTrajectory<T>::jointStatesCallback(const sensor_msgs::JointState::ConstPtr& states)
00180 {
00181 raw_joint_states_ = states;
00182 }
00183
00184 }
00185
00186 #endif