more optimization..

src/robot_dynamics/qros_robot_dynamic_py.cpp

@@ -53,6 +53,7 @@ PYBIND11_MODULE(_qros_robot_dynamic, m)
     py::class_<RDP>(m, "RobotDynamicsProvider")
         .def(py::init<const std::string&>())
         .def("publish_stiffness",&RDP::publish_stiffness)
+        .def("set_world_to_base_tf", &RDP::set_world_to_base_tf)
         .def("is_enabled",&RDP::is_enabled,"Returns true if the RobotDynamicProvider is enabled.")
         .def("get_topic_prefix",&RDP::get_topic_prefix);
 

src/robot_dynamics/qros_robot_dynamics_interface.cpp

@@ -47,8 +47,10 @@ RobotDynamicInterface::RobotDynamicInterface(ros::NodeHandle &publisher_nodehand
     last_stiffness_torque_(Vector3d::Zero()),
     last_stiffness_frame_pose_(0)
 {
+    // Strip potential leading slash
+    if(child_frame_id_.front() == '/'){child_frame_id_ = child_frame_id_.substr(1);}
+    if(parent_frame_id_.front() == '/'){parent_frame_id_ = parent_frame_id_.substr(1);}
     ROS_INFO_STREAM(ros::this_node::getName() + "::Initializing RobotDynamicInterface with prefix " + node_prefix);
-
     subscriber_cartesian_stiffness_ = subscriber_nodehandle.subscribe(node_prefix_ + "/get/cartesian_stiffness", 1, &RobotDynamicInterface::_callback_cartesian_stiffness, this);
 
 }
@@ -64,7 +66,7 @@ void RobotDynamicInterface::_callback_cartesian_stiffness(const geometry_msgs::W
     last_stiffness_torque_(2) = msg->wrench.torque.z;
 
     try {
-        const auto transform_stamped = tf_buffer_.lookupTransform(parent_frame_id_, child_frame_id_, ros::Time(0));
+        const auto transform_stamped = tf_buffer_.lookupTransform( parent_frame_id_, child_frame_id_, ros::Time(0));
         last_stiffness_frame_pose_ = _geometry_msgs_transform_to_dq(transform_stamped.transform);
     } catch (tf2::TransformException &ex) {
         ROS_WARN_STREAM(ros::this_node::getName() + "::" + ex.what());

src/robot_dynamics/qros_robot_dynamics_provider.cpp

@@ -40,8 +40,13 @@ RobotDynamicProvider::RobotDynamicProvider(ros::NodeHandle &nodehandle, const st
 RobotDynamicProvider::RobotDynamicProvider(ros::NodeHandle &publisher_nodehandle, ros::NodeHandle &subscriber_nodehandle, const std::string &node_prefix):
     node_prefix_(node_prefix),
     child_frame_id_(node_prefix + "_stiffness_frame"),
-    parent_frame_id_(node_prefix + "_base")
+    parent_frame_id_(node_prefix + "_base"),
+    world_to_base_tf_(0)
 {
+    // Strip potential leading slash
+    if(child_frame_id_.front() == '/'){child_frame_id_ = child_frame_id_.substr(1);}
+    if(parent_frame_id_.front() == '/'){parent_frame_id_ = parent_frame_id_.substr(1);}
+
     ROS_INFO_STREAM(ros::this_node::getName() + "::Initializing RobotDynamicProvider with prefix " + node_prefix);
     publisher_cartesian_stiffness_ = publisher_nodehandle.advertise<geometry_msgs::WrenchStamped>(node_prefix + "/get/cartesian_stiffness", 1);
 
@@ -63,6 +68,28 @@ geometry_msgs::Transform RobotDynamicProvider::_dq_to_geometry_msgs_transform(co
     return tf_msg;
 }
 
+void RobotDynamicProvider::set_world_to_base_tf(const DQ& world_to_base_tf)
+{
+    if(world_to_base_tf_==0)
+    {
+        world_to_base_tf_ = world_to_base_tf;
+        _publish_base_static_tf();
+    }else
+    {
+        throw std::runtime_error("The world to base transform has already been set");
+    }
+}
+
+void RobotDynamicProvider::_publish_base_static_tf()
+{
+    geometry_msgs::TransformStamped base_tf;
+    base_tf.transform = _dq_to_geometry_msgs_transform(world_to_base_tf_);
+    base_tf.header.stamp = ros::Time::now();
+    base_tf.header.frame_id = WORLD_FRAME_ID;
+    base_tf.child_frame_id = parent_frame_id_;
+    static_base_tf_broadcaster_.sendTransform(base_tf);
+
+}
 
 
 void RobotDynamicProvider::publish_stiffness(const DQ& base_to_stiffness, const Vector3d& force, const Vector3d& torque)

src/sas_robot_driver_franka_node.cpp

@@ -35,7 +35,7 @@
 
 #include <exception>
 #include <dqrobotics/utils/DQ_Math.h>
-//#include <dqrobotics/interfaces/json11/DQ_JsonReader.h>
+#include <dqrobotics/interfaces/json11/DQ_JsonReader.h>
 #include <sas_common/sas_common.h>
 #include <sas_conversions/eigen3_std_conversions.h>
 #include <sas_robot_driver/sas_robot_driver_ros.h>
@@ -121,7 +121,14 @@ int main(int argc, char **argv)
         ROS_INFO_STREAM(ros::this_node::getName()+"::Upper force threshold not set. Using default with value scalling.");
         franka_interface_configuration.upper_force_threshold = apply_scale_to_std_array(franka_interface_configuration.upper_force_threshold, upper_scale_factor);
     }
-
+    if(nh.hasParam(ros::this_node::getName()+"/robot_parameter_file_path"))
+    {
+        std::string robot_parameter_file_path;
+        sas::get_ros_param(nh,"/robot_parameter_file_path",robot_parameter_file_path);
+        ROS_INFO_STREAM(ros::this_node::getName()+"::Loading robot parameters from file: " + robot_parameter_file_path);
+        const auto robot = DQ_JsonReader::get_from_json<DQ_SerialManipulatorDH>(robot_parameter_file_path);
+        robot_driver_franka_configuration.robot_reference_frame = robot.get_reference_frame();
+    }else{ROS_INFO_STREAM(ros::this_node::getName()+"::Robot parameter file path not set. Robot Model Unknow.");}
 
     robot_driver_franka_configuration.interface_configuration = franka_interface_configuration;
 
@@ -134,7 +141,10 @@ int main(int argc, char **argv)
     // initialize the robot dynamic provider
     robot_driver_ros_configuration.robot_driver_provider_prefix = ros::this_node::getName();
     qros::RobotDynamicProvider robot_dynamic_provider(nh, robot_driver_ros_configuration.robot_driver_provider_prefix);
-
+    if(robot_driver_franka_configuration.robot_reference_frame!=0)
+    {
+        robot_dynamic_provider.set_world_to_base_tf(robot_driver_franka_configuration.robot_reference_frame);
+    }
 
     try
         {