diff --git a/src/MqttToRosNode.cpp b/src/MqttToRosNode.cpp index 8854df7be4d9047a145cd4a4fb2e23b7f19dbfe6..0709efe25d72a402d386605f76353425b771fbb3 100644 --- a/src/MqttToRosNode.cpp +++ b/src/MqttToRosNode.cpp @@ -23,11 +23,11 @@ void handleRobotConfig(const config::RobotConfig &robotConfig, const ros::NodeHa n.setParam("robot_speed_factor", robotConfig.speed()); } -void handleNewTrajectory(const plan::Trajectory &trajectory, const ros::Publisher &posePublisher) { +void handleNewTrajectory(const plan::Trajectory &protoTrajectory, const ros::Publisher &trajectoryPublisher) { - panda_mqtt_connector::Trajectory poseArray; + panda_mqtt_connector::Trajectory trajectory; - for (const auto &protoPose : trajectory.pose()) { + for (const auto &protoPose : protoTrajectory.pose()) { panda_mqtt_connector::Waypoint waypoint; waypoint.pose.orientation.w = protoPose.orientation().w(); waypoint.pose.orientation.x = protoPose.orientation().x(); @@ -48,16 +48,16 @@ void handleNewTrajectory(const plan::Trajectory &trajectory, const ros::Publishe ROS_WARN("Received pose has invalid mode!"); } - poseArray.waypoints.push_back(waypoint); + trajectory.waypoints.push_back(waypoint); } - poseArray.loop = trajectory.loop(); + trajectory.loop = protoTrajectory.loop(); - posePublisher.publish(poseArray); + trajectoryPublisher.publish(trajectory); } -void receiveMqttMessages(const ros::NodeHandle &n, const ros::Publisher &posePublisher) { +void receiveMqttMessages(const ros::NodeHandle &n, const ros::Publisher &trajectoryPublisher) { if (mqttUtil->ensureConnection()) { mqtt::const_message_ptr msg; if (mqttUtil->getClient().try_consume_message_for(&msg, std::chrono::milliseconds(500))) { @@ -71,7 +71,7 @@ void receiveMqttMessages(const ros::NodeHandle &n, const ros::Publisher &posePub const std::string tc_payload = msg->get_payload_str(); plan::Trajectory trajectoryConfig; trajectoryConfig.ParseFromString(tc_payload); - handleNewTrajectory(trajectoryConfig, posePublisher); + handleNewTrajectory(trajectoryConfig, trajectoryPublisher); } } } else { @@ -83,7 +83,7 @@ int main(int argc, char *argv[]) { ros::init(argc, argv, "mqtt_listener"); ros::NodeHandle n("panda_mqtt_connector"); - ros::Publisher posePublisher = n.advertise<panda_mqtt_connector::Trajectory>("poses", 1000); + ros::Publisher trajectoryPublisher = n.advertise<panda_mqtt_connector::Trajectory>("trajectory_update", 1000); std::string server; if (!n.getParam("server", server)) { @@ -111,7 +111,7 @@ int main(int argc, char *argv[]) { } while (ros::ok()) { - receiveMqttMessages(n, posePublisher); + receiveMqttMessages(n, trajectoryPublisher); ros::spinOnce(); } diff --git a/src/RobotStateProvider.cpp b/src/RobotStateProvider.cpp index c31bd089929fc00412b145ee401191acbe919103..cb743cd0dce4379e69e1a9412483c179abdc789e 100644 --- a/src/RobotStateProvider.cpp +++ b/src/RobotStateProvider.cpp @@ -14,7 +14,6 @@ #include "util/MqttUtil.h" namespace robot_state_provider { - bool export_to_log = false; /** * A map from the ROS element name to an MQTT topic @@ -30,49 +29,6 @@ namespace robot_state_provider { MqttUtil *mqttUtil = nullptr; -/* - * logs to its own file in /.ros/logs (configured in launch-file) - */ - void exportMessageToLog(const gazebo_msgs::LinkStates &msg) { - - ROS_INFO_STREAM("<<< LINK NAMES >>>"); - - for (int i = 0; i < msg.name.size(); i++) { - ROS_INFO_STREAM("[" << i << "] " << msg.name.at(i)); - } - - ROS_INFO_STREAM("<<< POSITIONS BY LINK >>>"); - - for (int i = 0; i < msg.pose.size(); i++) { - ROS_INFO_STREAM("[" << i << "] [x] " << msg.pose.at(i).position.x); - ROS_INFO_STREAM("[" << i << "] [y] " << msg.pose.at(i).position.y); - ROS_INFO_STREAM("[" << i << "] [z] " << msg.pose.at(i).position.z); - } - - ROS_INFO_STREAM("<<< ORIENTATIONS BY LINK >>>"); - - for (int i = 0; i < msg.pose.size(); i++) { - ROS_INFO_STREAM("[" << i << "] [w] " << msg.pose.at(i).orientation.w); - ROS_INFO_STREAM("[" << i << "] [x] " << msg.pose.at(i).orientation.x); - ROS_INFO_STREAM("[" << i << "] [y] " << msg.pose.at(i).orientation.y); - ROS_INFO_STREAM("[" << i << "] [z] " << msg.pose.at(i).orientation.z); - } - - ROS_INFO_STREAM("<<< LINEAR TWISTS BY LINK >>>"); - for (int i = 0; i < msg.twist.size(); i++) { - ROS_INFO_STREAM("[" << i << "] [x] " << msg.twist.at(i).linear.x); - ROS_INFO_STREAM("[" << i << "] [y] " << msg.twist.at(i).linear.y); - ROS_INFO_STREAM("[" << i << "] [z] " << msg.twist.at(i).linear.z); - } - - ROS_INFO_STREAM("<<< ANGULAR TWISTS BY LINK >>>"); - for (int i = 0; i < msg.twist.size(); i++) { - ROS_INFO_STREAM("[" << i << "] [x] " << msg.twist.at(i).angular.x); - ROS_INFO_STREAM("[" << i << "] [y] " << msg.twist.at(i).angular.y); - ROS_INFO_STREAM("[" << i << "] [z] " << msg.twist.at(i).angular.z); - } - } - std::vector<robot::RobotState> buildLinkStateMessages(const gazebo_msgs::LinkStates &msg) { std::vector<robot::RobotState> messages{}; @@ -159,9 +115,6 @@ namespace robot_state_provider { } if (current_redirect == 0) { - if (export_to_log) { - exportMessageToLog(msg); - } sendMqttMessages(msg); } current_redirect = (current_redirect + 1) % message_redirect_rate; diff --git a/src/TimedPlanner.cpp b/src/TimedPlanner.cpp index 34b8d3bb2a375b874be4a2095a33e3cce61ddb9f..91c3d3702bd2f9be5e6141eb77be41824f5dc515 100644 --- a/src/TimedPlanner.cpp +++ b/src/TimedPlanner.cpp @@ -50,7 +50,7 @@ constructPlate(std::vector<moveit_msgs::CollisionObject> &collision_objects, dou void TimedPlanner::loadCircularTrajectory(double radius, geometry_msgs::Point origin, int granularity) { - nextPoses = panda_mqtt_connector::Trajectory{}; + nextTrajectory = panda_mqtt_connector::Trajectory{}; double step_size_rad = 2 * M_PI / granularity; @@ -63,21 +63,22 @@ void TimedPlanner::loadCircularTrajectory(double radius, geometry_msgs::Point or tf2::Quaternion orientation; // the RPY constructor is deprecated orientation.setRPY(0, M_PI, next_rad); - panda_mqtt_connector::Waypoint nextPose{}; - nextPose.pose.orientation = tf2::toMsg(orientation); - nextPose.pose.position.x = origin.x + radius * sin(next_rad); - nextPose.pose.position.y = origin.y + radius * cos(next_rad); - nextPose.pose.position.z = origin.z; + panda_mqtt_connector::Waypoint nextWaypoint{}; + nextWaypoint.pose.orientation = tf2::toMsg(orientation); + nextWaypoint.pose.position.x = origin.x + radius * sin(next_rad); + nextWaypoint.pose.position.y = origin.y + radius * cos(next_rad); + nextWaypoint.pose.position.z = origin.z; + nextWaypoint.mode = TrajectoryUtil::FLUID_PATH; - nextPoses->waypoints.push_back(nextPose); + nextTrajectory->waypoints.push_back(nextWaypoint); } } bool TimedPlanner::loadWaypoints() { - if (nextPoses.is_initialized()) { - poses = nextPoses.get(); - nextPoses.reset(); + if (nextTrajectory.is_initialized()) { + currentTrajectory = nextTrajectory.get(); + nextTrajectory.reset(); return true; } else { return false; @@ -113,18 +114,14 @@ void TimedPlanner::doMotion(const ros::NodeHandle &node_handle, moveit::planning loadWaypoints(); - while (poses.loop) { - for (auto &waypoint : poses.waypoints) { - - ROS_INFO_STREAM("Planning the next trajectory in " << waypoint.mode); + while (currentTrajectory.loop) { + for (auto &waypoint : currentTrajectory.waypoints) { moveit::planning_interface::MoveGroupInterface::Plan plan; - ROS_ERROR_STREAM(waypoint); - if (TrajectoryUtil::computePathToPose(group, plan, waypoint.pose, waypoint.mode, 0.8, 0.8)) { - if (nextPoses.is_initialized()) { + if (nextTrajectory.is_initialized()) { return; } @@ -137,7 +134,8 @@ void TimedPlanner::doMotion(const ros::NodeHandle &node_handle, moveit::planning ROS_ERROR_STREAM("Invalid motion speed factor " << motionSpeedFactor << ". Must be in (0,1]."); return; } - ROS_INFO_STREAM("Moving to the next waypoint with speed factor " << motionSpeedFactor); + ROS_INFO_STREAM( + "Moving to the next waypoint with speed factor " << motionSpeedFactor << " in mode " << waypoint.mode); TrajectoryUtil::applyMotionSpeedFactor(trajectory, motionSpeedFactor); moveit_msgs::MoveItErrorCodes errorCode = group.execute(trajectory); if (errorCode.val != errorCode.SUCCESS) { @@ -145,15 +143,16 @@ void TimedPlanner::doMotion(const ros::NodeHandle &node_handle, moveit::planning } } } else { - ROS_ERROR_STREAM("The planner was unable to compute a path to the pose with the given settings."); + ROS_ERROR_STREAM( + "Unable to compute " << waypoint.mode << " path to the next waypoint." << std::endl << waypoint); } } } } -void TimedPlanner::newPoseCallback(const panda_mqtt_connector::Trajectory::ConstPtr &msg) { - ROS_INFO_STREAM("Received new pose list with " << msg->waypoints.size() << " poses."); - nextPoses = *msg; +void TimedPlanner::newTrajectoryCallback(const panda_mqtt_connector::Trajectory::ConstPtr &msg) { + ROS_INFO_STREAM("Received new trajectory with " << msg->waypoints.size() << " waypoints."); + nextTrajectory = *msg; } TimedPlanner::TimedPlanner(moveit::planning_interface::MoveGroupInterface &group, const double defaultVelocity, @@ -162,51 +161,48 @@ TimedPlanner::TimedPlanner(moveit::planning_interface::MoveGroupInterface &group ros::NodeHandle node_handle("panda_mqtt_connector"); - std::string defaultTrajectory = "<none>"; + std::string defaultTrajectory = "<no value provided>"; node_handle.getParam("default_trajectory", defaultTrajectory); - geometry_msgs::Pose base_pose = group.getCurrentPose().pose; - - if (loadWaypoints()) { - ROS_INFO_STREAM("Ignoring default waypoints because poses were already received as a message"); - } else if (defaultTrajectory == "circle") { + if (defaultTrajectory == "circle") { ROS_INFO_STREAM("loading default trajectory 'circle'"); geometry_msgs::Point origin; origin.z = .3; loadCircularTrajectory(0.6, origin, 20); } else if (defaultTrajectory == "square") { - loadSquareTrajectory(base_pose); - + ROS_INFO_STREAM("loading default trajectory 'square'"); + loadSquareTrajectory(); } else { - ROS_WARN_STREAM("not loading a default trajectory! provided trajectory: " + defaultTrajectory); + ROS_WARN_STREAM("Not loading a default trajectory! There is no '" + defaultTrajectory + "'."); } } -void TimedPlanner::loadSquareTrajectory(geometry_msgs::Pose base_pose) { - ROS_INFO_STREAM("loading default trajectory 'square'"); - // choose a default trajectory - base_pose.position.x = 0.4; - base_pose.position.y = 0.4; - base_pose.position.z = 0.3; - panda_mqtt_connector::Waypoint target_pose_1; - target_pose_1.pose = base_pose; - target_pose_1.mode = TrajectoryUtil::FLUID_PATH; - panda_mqtt_connector::Waypoint target_pose_2{target_pose_1}; - panda_mqtt_connector::Waypoint target_pose_3{target_pose_1}; - panda_mqtt_connector::Waypoint target_pose_4{target_pose_1}; - - poses.waypoints.push_back(target_pose_1); - - target_pose_2.pose.position.y *= -1; - poses.waypoints.push_back(target_pose_2); - - target_pose_3.pose.position.x *= -1; - target_pose_3.pose.position.y *= -1; - poses.waypoints.push_back(target_pose_3); - - target_pose_4.pose.position.x *= -1; - poses.waypoints.push_back(target_pose_4); +void TimedPlanner::loadSquareTrajectory() { + panda_mqtt_connector::Waypoint waypoint_1; + waypoint_1.pose.position.x = 0.4; + waypoint_1.pose.position.y = 0.4; + waypoint_1.pose.position.z = 0.3; + waypoint_1.pose.orientation.w = 0; + waypoint_1.pose.orientation.x = 0.7071067811865476; + waypoint_1.pose.orientation.y = 0.7071067811865476; + waypoint_1.pose.orientation.z = 0; + waypoint_1.mode = TrajectoryUtil::FLUID_PATH; + panda_mqtt_connector::Waypoint waypoint_2{waypoint_1}; + panda_mqtt_connector::Waypoint waypoint_3{waypoint_1}; + panda_mqtt_connector::Waypoint waypoint_4{waypoint_1}; + + currentTrajectory.waypoints.push_back(waypoint_1); + + waypoint_2.pose.position.y *= -1; + currentTrajectory.waypoints.push_back(waypoint_2); + + waypoint_3.pose.position.x *= -1; + waypoint_3.pose.position.y *= -1; + currentTrajectory.waypoints.push_back(waypoint_3); + + waypoint_4.pose.position.x *= -1; + currentTrajectory.waypoints.push_back(waypoint_4); } @@ -228,7 +224,8 @@ int main(int argc, char **argv) { TimedPlanner planner(group, .5, TrajectoryUtil::FLUID_PATH); - ros::Subscriber sub = node_handle.subscribe("poses", 1000, &TimedPlanner::newPoseCallback, &planner); + ros::Subscriber sub = node_handle.subscribe("trajectory_update", 1000, &TimedPlanner::newTrajectoryCallback, + &planner); // add the ground plate std::vector<moveit_msgs::CollisionObject> collision_objects; diff --git a/src/TimedPlanner.h b/src/TimedPlanner.h index 2031794fd6de26a254c9c46e68055d16841d5092..7c5ad38ea04c88f840ad0e75a9de795668e3529d 100644 --- a/src/TimedPlanner.h +++ b/src/TimedPlanner.h @@ -11,31 +11,32 @@ #include "panda_mqtt_connector/Trajectory.h" +using moveit::planning_interface::MoveGroupInterface; + class TimedPlanner { public: const double default_velocity = 1.0; const std::string default_planning_mode = TrajectoryUtil::FLUID_PATH; - static std::vector<moveit_msgs::RobotTrajectory> split(moveit::planning_interface::MoveGroupInterface &group, - const moveit::planning_interface::MoveGroupInterface::Plan &plan); + static std::vector<moveit_msgs::RobotTrajectory> + split(MoveGroupInterface &group, const MoveGroupInterface::Plan &plan); - void doMotion(const ros::NodeHandle &node_handle, moveit::planning_interface::MoveGroupInterface &group); + void doMotion(const ros::NodeHandle &node_handle, MoveGroupInterface &group); - TimedPlanner(moveit::planning_interface::MoveGroupInterface &group, double defaultVelocity, - std::string defaultPlanningMode); + TimedPlanner(MoveGroupInterface &group, double defaultVelocity, std::string defaultPlanningMode); - void newPoseCallback(const panda_mqtt_connector::Trajectory::ConstPtr &msg); + void newTrajectoryCallback(const panda_mqtt_connector::Trajectory::ConstPtr &msg); private: - panda_mqtt_connector::Trajectory poses; - boost::optional<panda_mqtt_connector::Trajectory> nextPoses; - - void loadCircularTrajectory(double radius, geometry_msgs::Point origin, int granularity); + panda_mqtt_connector::Trajectory currentTrajectory; + boost::optional<panda_mqtt_connector::Trajectory> nextTrajectory; bool loadWaypoints(); - void loadSquareTrajectory(geometry_msgs::Pose base_pose); + void loadSquareTrajectory(); + + void loadCircularTrajectory(double radius, geometry_msgs::Point origin, int granularity); };