ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question

Revision history [back]

click to hide/show revision 1
initial version

I have resolved this for my needs by adding

in robot_trajectory.cpp

at line 284ish

        if (waypoints_[i]->hasVelocities()){
        const std::vector<std::string> names =  mdof[j]->getVariableNames();
        const double* velocities =  waypoints_[i]->getJointVelocities(mdof[j]);
         geometry_msgs::Twist point_velocity;
        for(std::size_t ii = 0; ii < names.size(); ++ii){
          if(names[ii].find("/x") != std::string::npos)
          {
            ROS_INFO_STREAM("multi dof joint x velocity: "<<velocities[ii]);
            point_velocity.linear.x = velocities[ii];
          }
          else if(names[ii].find("/y") != std::string::npos)
          {
            ROS_INFO_STREAM("multi dof joint y velocity: "<<velocities[ii]);
            point_velocity.linear.y = velocities[ii];
          }
          else if(names[ii].find("/z") != std::string::npos)
          {
            ROS_INFO_STREAM("multi dof joint z velocity: "<<velocities[ii]);
            point_velocity.linear.z = velocities[ii];
          }
          else if(names[ii].find("/theta") != std::string::npos)
          {
            ROS_INFO_STREAM("multi dof joint theta velocity: "<<velocities[ii]);
            point_velocity.angular.z = velocities[ii];
          }
          else
          {
            ROS_INFO_STREAM("velocity name: "<<names[ii]);
          }
        }
        trajectory.multi_dof_joint_trajectory.points[i].velocities.push_back(point_velocity);
       }

in trajectory_execution_manager.cpp

at line 863

        if (!trajectory.multi_dof_joint_trajectory.points[j].velocities.empty())
        {
          parts[i].multi_dof_joint_trajectory.points[j].velocities.resize(bijection.size());

          parts[i].multi_dof_joint_trajectory.points[j].velocities[0].linear.x =
            trajectory.multi_dof_joint_trajectory.points[j].velocities[0].linear.x * execution_velocity_scaling_;

          parts[i].multi_dof_joint_trajectory.points[j].velocities[0].linear.y =
            trajectory.multi_dof_joint_trajectory.points[j].velocities[0].linear.y * execution_velocity_scaling_;

          parts[i].multi_dof_joint_trajectory.points[j].velocities[0].linear.z =
            trajectory.multi_dof_joint_trajectory.points[j].velocities[0].linear.z * execution_velocity_scaling_;

          parts[i].multi_dof_joint_trajectory.points[j].velocities[0].angular.x =
            trajectory.multi_dof_joint_trajectory.points[j].velocities[0].angular.x * execution_velocity_scaling_;

          parts[i].multi_dof_joint_trajectory.points[j].velocities[0].angular.y =
            trajectory.multi_dof_joint_trajectory.points[j].velocities[0].angular.y * execution_velocity_scaling_;

          parts[i].multi_dof_joint_trajectory.points[j].velocities[0].angular.z =
            trajectory.multi_dof_joint_trajectory.points[j].velocities[0].angular.z * execution_velocity_scaling_;
        }