32 #include <sensor_msgs/JointState.h> 33 #include <trajectory_msgs/JointTrajectory.h> 34 #include <ypspur_ros/JointPositionControl.h> 50 std::map<std::string, double>
state_;
57 for (
size_t i = 0; i < msg->name.size(); i++)
59 state_[msg->name[i]] = msg->position[i];
67 if (msg->joint_names.size() != cmd_prev.joint_names.size())
71 for (
unsigned int i = 0; i < msg->joint_names.size(); i++)
73 if (msg->joint_names[i].compare(cmd_prev.joint_names[i]) != 0)
79 if (msg->positions.size() != cmd_prev.positions.size())
83 for (
unsigned int i = 0; i < msg->positions.size(); i++)
85 if (msg->positions[i] != cmd_prev.positions[i])
91 if (msg->velocities.size() != cmd_prev.velocities.size())
95 for (
unsigned int i = 0; i < msg->velocities.size(); i++)
97 if (msg->velocities[i] != cmd_prev.velocities[i])
103 if (msg->accelerations.size() != cmd_prev.accelerations.size())
107 for (
unsigned int i = 0; i < msg->accelerations.size(); i++)
109 if (msg->accelerations[i] != cmd_prev.accelerations[i])
119 trajectory_msgs::JointTrajectory cmd;
120 cmd.header = msg->header;
121 cmd.joint_names = msg->joint_names;
122 cmd.points.resize(1);
123 cmd.points[0].velocities.resize(msg->positions.size());
124 cmd.points[0].positions = msg->positions;
125 cmd.points[0].accelerations.resize(msg->positions.size());
129 for (
auto&
p : msg->positions)
131 float t = fabs(
p - state_[msg->joint_names[i]]) / msg->velocities[i];
148 nh_,
"joint_position",
153 pub_joint_ = compat::advertise<trajectory_msgs::JointTrajectory>(
154 nh_,
"joint_trajectory",
155 pnh_,
"joint_trajectory", 5,
false);
157 pnh_.param(
"accel", accel_, 0.3);
158 pnh_.param(
"skip_same", skip_same_,
true);
162 int main(
int argc,
char* argv[])
164 ros::init(argc, argv,
"joint_position_to_joint_trajectory");
std::map< std::string, double > state_
ros::Subscriber sub_joint_state_
int main(int argc, char *argv[])
void publish(const boost::shared_ptr< M > &message) const
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
ros::Subscriber sub_joint_
void cbJointState(const sensor_msgs::JointState::ConstPtr &msg)
ros::Publisher pub_joint_
ROSCPP_DECL void spin(Spinner &spinner)
void cbJointPosition(const ypspur_ros::JointPositionControl::ConstPtr &msg)
ypspur_ros::JointPositionControl cmd_prev
double p(YPSpur_param id, enum motor_id motor)
ros::Subscriber subscribe(ros::NodeHandle &nh_new, const std::string &topic_new, ros::NodeHandle &nh_old, const std::string &topic_old, uint32_t queue_size, void(T::*fp)(M) const, T *obj, const ros::TransportHints &transport_hints=ros::TransportHints())