41 #include <pr2_controllers_msgs/JointTrajectoryAction.h> 42 #include <pr2_controllers_msgs/JointTrajectoryControllerState.h> 46 #include <urdf_model/joint.h> 58 as_(
ros::NodeHandle(),
"joint_trajectory_generator",
61 ac_(
"joint_trajectory_action"),
73 std::string xml_string;
75 if (!nh_toplevel.
getParam(std::string(
"/robot_description"), xml_string))
76 throw ros::Exception(
"Could not find paramter robot_description on parameter server");
89 ROS_INFO(
"%s: Initialized",name.c_str());
92 void jointStateCb(
const pr2_controllers_msgs::JointTrajectoryControllerStateConstPtr& state){
93 boost::mutex::scoped_lock lock(
mutex_);
94 for(
unsigned int i = 0; i < state->joint_names.size(); ++i){
95 current_state_[state->joint_names[i]] = state->actual.positions[i];
100 pr2_controllers_msgs::JointTrajectoryGoal
createGoal(
const pr2_controllers_msgs::JointTrajectoryGoal& goal){
101 pr2_controllers_msgs::JointTrajectoryGoal new_goal;
102 new_goal.trajectory.header = goal.trajectory.header;
103 new_goal.trajectory.joint_names = goal.trajectory.joint_names;
105 size_t n_traj_points = goal.trajectory.points.size(),
106 n_joint_names = goal.trajectory.joint_names.size();
110 new_goal.trajectory.points.resize(n_traj_points + 1);
113 for(
size_t i=0; i<n_traj_points+1; i++) {
114 new_goal.trajectory.points[i].positions.resize(n_joint_names);
118 boost::mutex::scoped_lock lock(
mutex_);
120 for(
unsigned int i = 0; i < n_joint_names; ++i){
123 ROS_FATAL_STREAM(
"Joint names in goal and controller don't match. Something is very wrong. Goal joint name: "<<new_goal.trajectory.joint_names[i]);
124 throw std::runtime_error(
"Joint names in goal and controller don't match. Something is very wrong.");
126 new_goal.trajectory.points[0].positions[i] =
current_state_[new_goal.trajectory.joint_names[i]];
129 #if URDFDOM_1_0_0_API 130 urdf::JointConstSharedPtr joint =
robot_model_.getJoint(new_goal.trajectory.joint_names[i]);
136 double goal_position = goal.trajectory.points[0].positions[i],
137 current_position = new_goal.trajectory.points[0].positions[i];
139 if(joint->type == urdf::Joint::REVOLUTE) {
141 }
else if(joint->type == urdf::Joint::CONTINUOUS) {
144 ROS_WARN(
"Unknown joint type in joint trajectory. This joint might not be unwrapped properly. Supported joint types are urdf::Joint::REVOLUTE and urdf::Joint::CONTINUOUS");
149 for(
unsigned int j=0; j < n_traj_points; j++) {
150 new_goal.trajectory.points[j+1].time_from_start = goal.trajectory.points[j].time_from_start;
151 new_goal.trajectory.points[j+1].positions[i] = goal.trajectory.points[j].positions[i] + offset;
160 g.
generate(new_goal.trajectory, new_goal.trajectory);
165 void executeCb(
const pr2_controllers_msgs::JointTrajectoryGoalConstPtr& goal){
168 pr2_controllers_msgs::JointTrajectoryGoal full_goal;
202 pr2_controllers_msgs::JointTrajectoryResultConstPtr result =
ac_.
getResult();
217 as_.
setAborted(*result,
"Unknown result from joint_trajectory_action");
221 void feedbackCb(
const pr2_controllers_msgs::JointTrajectoryFeedbackConstPtr& feedback){
238 int main(
int argc,
char** argv){
239 ros::init(argc, argv,
"joint_trajectory_generator_node");
boost::shared_ptr< const Goal > acceptNewGoal()
void publishFeedback(const FeedbackConstPtr &feedback)
bool waitForServer(const ros::Duration &timeout=ros::Duration(0, 0)) const
URDF_EXPORT bool initString(const std::string &xmlstring)
ros::Subscriber state_sub_
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
pr2_controllers_msgs::JointTrajectoryGoal createGoal(const pr2_controllers_msgs::JointTrajectoryGoal &goal)
bool waitForResult(const ros::Duration &timeout=ros::Duration(0, 0))
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
ROSCPP_DECL const std::string & getName()
void setSucceeded(const Result &result=Result(), const std::string &text=std::string(""))
boost::function< void()> SimpleActiveCallback
void setPreempted(const Result &result=Result(), const std::string &text=std::string(""))
actionlib::SimpleActionClient< pr2_controllers_msgs::JointTrajectoryAction > JTAC
#define ROS_FATAL_STREAM(args)
actionlib::SimpleActionServer< pr2_controllers_msgs::JointTrajectoryAction > JTAS
bool param(const std::string ¶m_name, T ¶m_val, const T &default_val) const
void executeCb(const pr2_controllers_msgs::JointTrajectoryGoalConstPtr &goal)
JointTrajectoryGenerator(std::string name)
bool isPreemptRequested()
int main(int argc, char **argv)
#define ROS_DEBUG_STREAM(args)
void generate(const trajectory_msgs::JointTrajectory &traj_in, trajectory_msgs::JointTrajectory &traj_out)
boost::function< void(const SimpleClientGoalState &state, const ResultConstPtr &result)> SimpleDoneCallback
void setAborted(const Result &result=Result(), const std::string &text=std::string(""))
void sendGoal(const Goal &goal, SimpleDoneCallback done_cb=SimpleDoneCallback(), SimpleActiveCallback active_cb=SimpleActiveCallback(), SimpleFeedbackCallback feedback_cb=SimpleFeedbackCallback())
void jointStateCb(const pr2_controllers_msgs::JointTrajectoryControllerStateConstPtr &state)
bool getParam(const std::string &key, std::string &s) const
void feedbackCb(const pr2_controllers_msgs::JointTrajectoryFeedbackConstPtr &feedback)
SimpleClientGoalState getState() const
#define ROS_ERROR_STREAM(args)
ResultConstPtr getResult() const
ROSCPP_DECL void spinOnce()
bool isNewGoalAvailable()
def shortest_angular_distance(from_angle, to_angle)
std::map< std::string, double > current_state_