diff --git a/proto/dataconfig.proto b/proto/dataconfig.proto
index 3ec464ef47c245a54b331d990dd137ff1eebe17c..3478e611e3447c9c28c9691c73c60e21d1ae55b2 100644
--- a/proto/dataconfig.proto
+++ b/proto/dataconfig.proto
@@ -1,13 +1,13 @@
-syntax = "proto2";
+syntax = "proto3";
package config;
message DataConfig {
- required bool enablePosition = 1;
- required bool enableOrientation = 2;
- required bool enableTwistLinear = 3;
- required bool enableTwistAngular = 4;
+ bool enablePosition = 1;
+ bool enableOrientation = 2;
+ bool enableTwistLinear = 3;
+ bool enableTwistAngular = 4;
- required int32 publishRate = 5;
+ int32 publishRate = 5;
}
\ No newline at end of file
diff --git a/proto/linkstate.proto b/proto/linkstate.proto
index df7fd3a1c79959ca161a9ec83b42d4d96319b2c8..c07d9d86f4b54b2264601c0723e03194a3e2c6be 100644
--- a/proto/linkstate.proto
+++ b/proto/linkstate.proto
@@ -1,38 +1,38 @@
-syntax = "proto2";
+syntax = "proto3";
package panda;
message PandaLinkState {
- required string name = 1;
+ string name = 1;
message Position {
- required float positionX = 1;
- required float positionY = 2;
- required float positionZ = 3;
+ float positionX = 1;
+ float positionY = 2;
+ float positionZ = 3;
}
message Orientation {
- required float orientationX = 1;
- required float orientationY = 2;
- required float orientationZ = 3;
- required float orientationW = 4;
+ float orientationX = 1;
+ float orientationY = 2;
+ float orientationZ = 3;
+ float orientationW = 4;
}
message TwistLinear {
- required float twistLinearX = 1;
- required float twistLinearY = 2;
- required float twistLinearZ = 3;
+ float twistLinearX = 1;
+ float twistLinearY = 2;
+ float twistLinearZ = 3;
}
message TwistAngular {
- required float twistAngularX = 1;
- required float twistAngularY = 2;
- required float twistAngularZ = 3;
+ float twistAngularX = 1;
+ float twistAngularY = 2;
+ float twistAngularZ = 3;
}
- optional Position pos = 2;
- optional Orientation orient = 3;
- optional TwistLinear tl = 4;
- optional TwistAngular ta = 5;
+ Position pos = 2;
+ Orientation orient = 3;
+ TwistLinear tl = 4;
+ TwistAngular ta = 5;
}
\ No newline at end of file
diff --git a/proto/robotconfig.proto b/proto/robotconfig.proto
index 30dbbb75a3e8e8b3ae95afe2efb7397338d8bab6..20efd38480dba9e556e584827052317e76b2a932 100644
--- a/proto/robotconfig.proto
+++ b/proto/robotconfig.proto
@@ -1,8 +1,8 @@
-syntax = "proto2";
+syntax = "proto3";
package config;
message RobotConfig {
- required double speed = 1;
+ double speed = 1;
}
\ No newline at end of file
diff --git a/src/MqttToRosNode.cpp b/src/MqttToRosNode.cpp
index a2138029dc12390258c90a88b2536c87823d8b30..d2dc748b60c4636c352b1e040aaf74d8adbbac33 100644
--- a/src/MqttToRosNode.cpp
+++ b/src/MqttToRosNode.cpp
@@ -46,14 +46,14 @@ void handleDataConfig(config::DataConfig dataConfig, ros::NodeHandle n)
<< " -- enablePosition: " << dataConfig.enableposition()
<< " -- enableOrientation: " << dataConfig.enableorientation()
<< " -- enableTwistLinear: " << dataConfig.enabletwistlinear()
- << " -- enableTwistAngular: " << dataConfig.has_enabletwistangular()
+ << " -- enableTwistAngular: " << dataConfig.enabletwistangular()
<< std::endl;
n.setParam("data_publish_rate", dataConfig.publishrate());
n.setParam("data_enable_position", dataConfig.enableposition());
n.setParam("data_enable_orientation", dataConfig.enableorientation());
n.setParam("data_enable_twist_linear", dataConfig.enabletwistlinear());
- n.setParam("data_enable_twist_angular", dataConfig.has_enabletwistangular());
+ n.setParam("data_enable_twist_angular", dataConfig.enabletwistangular());
}
void receiveMqttMessages(ros::NodeHandle n)
diff --git a/src/TimedCartesianPlanner.cpp b/src/TimedCartesianPlanner.cpp
index cba8846281d729fc9e047c8ac4ccd817ebee69cc..fac7483054ac91f1c2947792a7d7452d9071aa88 100644
--- a/src/TimedCartesianPlanner.cpp
+++ b/src/TimedCartesianPlanner.cpp
@@ -16,6 +16,25 @@ const double default_velocity = 1.0;
bool isInitialized = false;
+// adjust velocities, accelerations (= sqrt of velocity) and time_from_start
+void manipulateVelocity(double velocity, moveit_msgs::RobotTrajectory &trajectory_msg) {
+
+ for (int i = 0; i < trajectory_msg.joint_trajectory.points.size(); i++) {
+ double new_tfs = 1 / velocity;
+ trajectory_msg.joint_trajectory.points[i].time_from_start.operator*=(new_tfs);
+ for (int j = 0; j < trajectory_msg.joint_trajectory.points.at(i).velocities.size(); j++) {
+ if (trajectory_msg.joint_trajectory.points.at(i).velocities.at(j) != 0.0) {
+ trajectory_msg.joint_trajectory.points.at(i).velocities.at(j) *= velocity;
+ }
+ }
+
+ for (int j = 0; j < trajectory_msg.joint_trajectory.points.at(i).accelerations.size(); j++) {
+ trajectory_msg.joint_trajectory.points.at(i).accelerations.at(j) =
+ sqrt(std::abs(trajectory_msg.joint_trajectory.points.at(i).accelerations.at(j)));
+ }
+ }
+}
+
void initRawTrajectory(ros::NodeHandle node_handle, moveit::planning_interface::MoveGroupInterface *group) {
geometry_msgs::Pose target_pose_1 = group->getCurrentPose().pose;
geometry_msgs::Pose target_pose_2 = group->getCurrentPose().pose;
@@ -84,7 +103,6 @@ void computeTrajectory(moveit::planning_interface::MoveGroupInterface &group, in
bool success = iptp.computeTimeStamps(rt);
ROS_INFO("Computed time stamp %s", success ? "SUCCEDED" : "FAILED");
rt.getRobotTrajectoryMsg(trajectory_msg);
- plan.trajectory_ = trajectory_msg;
ROS_INFO("Visualizing cartesian plan (%.2f%% acheived)", fraction * 100.0);
}
@@ -111,6 +129,7 @@ int main(int argc, char **argv) {
text_pose.translation().z() = 1.75;
visual_tools.publishText(text_pose, "cartesian planner node", rvt::WHITE, rvt::XLARGE);
visual_tools.trigger();
+ //planning_scene::PlanningScene;
// Initialize start state of robot and target trajectory.
moveit::planning_interface::MoveGroupInterface group("panda_arm");
@@ -131,24 +150,10 @@ int main(int argc, char **argv) {
moveit::planning_interface::MoveGroupInterface::Plan plan;
moveit_msgs::RobotTrajectory trajectory_msg;
computeTrajectory(group, i, plan, trajectory_msg);
-
- // adjust velocities, accelerations (= sqrt of velocity) and time_from_start
- for (int i = 0; i < trajectory_msg.joint_trajectory.points.size(); i++) {
- double new_tfs = 1 / velocity;
- trajectory_msg.joint_trajectory.points[i].time_from_start.operator*=(new_tfs);
- for (int j = 0; j < trajectory_msg.joint_trajectory.points.at(i).velocities.size(); j++) {
- if (trajectory_msg.joint_trajectory.points.at(i).velocities.at(j) != 0.0) {
- trajectory_msg.joint_trajectory.points.at(i).velocities.at(j) *= velocity;
- }
- }
-
- for (int j = 0; j < trajectory_msg.joint_trajectory.points.at(i).accelerations.size(); j++) {
- trajectory_msg.joint_trajectory.points.at(i).accelerations.at(j) =
- sqrt(std::abs(trajectory_msg.joint_trajectory.points.at(i).accelerations.at(j)));
- }
- }
+ manipulateVelocity(velocity, trajectory_msg);
// execute one step of the trajectory
+ plan.trajectory_ = trajectory_msg;
group.execute(plan);
// make sure the robot moves in an infinite circle