Skip to content
Snippets Groups Projects
Commit e35a44f6 authored by Johannes Mey's avatar Johannes Mey
Browse files

cleanup

parent 37947af3
Branches
No related tags found
No related merge requests found
Show whitespace changes
Inline Side-by-side
Showing
with 10 additions and 440 deletions
.devcontainer/Dockerfile deleted 100644 → 0
+ 0
− 6
View file @ 37947af3
FROM osrf/ros:melodic-desktop-full-bionic
RUN apt-get update && apt-get install -q -y \
openssh-client
RUN echo 'source /opt/ros/melodic/setup.bash' >> /root/.bashrc
\ No newline at end of file
.devcontainer/devcontainer.json deleted 100644 → 0
+ 0
− 21
View file @ 37947af3
{
"name": "panda_simulation-dev",
"dockerFile": "Dockerfile",
"extensions": [
"ms-vscode.cpptools",
"ms-iot.vscode-ros"
],
"runArgs": [
"--cap-add=SYS_PTRACE",
"--security-opt",
"seccomp=unconfined",
"-v",
"${env:HOME}${env:USERPROFILE}/.ssh:/root/.ssh"
],
"settings": {
"terminal.integrated.shell.linux": "/bin/bash"
},
"postCreateCommand": "bash /catkin_ws/src/panda_simulation/scripts/docker-setup.sh",
"workspaceMount": "src=/Users/pekel/code/ros/panda_simulation,dst=/catkin_ws/src/panda_simulation,type=bind,consistency=cached",
"workspaceFolder": "/catkin_ws"
}
\ No newline at end of file
README.md
+ 10
− 72
View file @ e35a44f6
# panda_simulation # panda_mqtt_connector
![Panda in Gazebo](assets/panda-in-gazebo.png?raw=true "Panda in Gazebo")
This package was written for ROS melodic running under Ubuntu 18.04. Run the following commands to make sure that all additional packages are installed:
```
mkdir -p catkin_ws/src
cd catkin_ws/src
git clone https://github.com/erdalpekel/panda_simulation.git
git clone https://github.com/erdalpekel/panda_moveit_config.git
git clone --branch simulation https://github.com/erdalpekel/franka_ros.git
cd ..
sudo apt-get install libboost-filesystem-dev
rosdep install --from-paths src --ignore-src -y --skip-keys libfranka
cd ..
```
It is also important that you build the *libfranka* library from source and pass its directory to *catkin_make* when building this ROS package as described in [this tutorial](https://frankaemika.github.io/docs/installation.html#building-from-source).
Currently it includes a controller parameter config file and a launch file to launch the [Gazebo](http://gazebosim.org) simulation environment and the Panda robot from FRANKA EMIKA in it with the necessary controllers.
Build the catkin workspace and run the simulation:
```
catkin_make -j4 -DCMAKE_BUILD_TYPE=Release -DFranka_DIR:PATH=/path/to/libfranka/build
source devel/setup.bash
roslaunch panda_simulation simulation.launch
```
Depending on your operating systems language you might need to export the numeric type so that rviz can read the floating point numbers in the robot model correctly:
```
export LC_NUMERIC="en_US.UTF-8"
```
Otherwise, the robot will appear in rviz in a collapsed state.
You can see the full explanation in my [blog post](https://erdalpekel.de/?p=55).
## Installing Eclipse Paho MQTT-Client for C++ ## Installing Eclipse Paho MQTT-Client for C++
...@@ -54,32 +18,6 @@ $ sudo apt-get install doxygen graphviz ...@@ -54,32 +18,6 @@ $ sudo apt-get install doxygen graphviz
$ sudo apt-get install libcppunit-dev $ sudo apt-get install libcppunit-dev
``` ```
Before building the C++ library, first, build and install the Paho C library:
```
$ git clone https://github.com/eclipse/paho.mqtt.c.git
$ cd paho.mqtt.c
$ git checkout v1.3.1
$ cmake -Bbuild -H. -DPAHO_WITH_SSL=ON -DPAHO_ENABLE_TESTING=OFF
$ sudo cmake --build build/ --target install
$ sudo ldconfig
```
Build and install the C++ library:
```
$ git clone https://github.com/eclipse/paho.mqtt.cpp
$ cd paho.mqtt.cpp
$ cmake -Bbuild -H. -DPAHO_BUILD_DOCUMENTATION=TRUE -DPAHO_BUILD_SAMPLES=TRUE
$ sudo cmake --build build/ --target install
$ sudo ldconfig
```
(source: https://github.com/eclipse/paho.mqtt.cpp/blob/master/README.md)
* see gitlab-CI for more details
## How to start it ## How to start it
### Installation ### Installation
...@@ -91,8 +29,8 @@ $ sudo ldconfig ...@@ -91,8 +29,8 @@ $ sudo ldconfig
### Start the System ### Start the System
* Production-Mode: roslaunch panda_simulation simulation_rosrag.launch * Production-Mode: roslaunch panda_mqtt_connector simulation_rosrag.launch
* Test-Mode: roslaunch panda_simulation simulation_rosrag_test.launch * Test-Mode: roslaunch panda_mqtt_connector simulation_rosrag_test.launch
* Starts an additional node which listens to MQTT channels for testing * Starts an additional node which listens to MQTT channels for testing
* Channel 1: configtest -> manipulates messages (switch on/off the message parts) and the publish-rate * Channel 1: configtest -> manipulates messages (switch on/off the message parts) and the publish-rate
* Channel 2: modetest -> changes planning mode to "fluid" * Channel 2: modetest -> changes planning mode to "fluid"
...@@ -106,7 +44,7 @@ $ sudo ldconfig ...@@ -106,7 +44,7 @@ $ sudo ldconfig
#### General #### General
* Channel: dataconfig * Channel: dataconfig
* Protobuff-message format defined in https://git-st.inf.tu-dresden.de/ceti/ros/panda_simulation/-/blob/development/morse/proto/dataconfig.proto * Protobuff-message format defined in https://git-st.inf.tu-dresden.de/ceti/ros/panda_mqtt_connector/-/blob/development/morse/proto/dataconfig.proto
* Defines the data submitted from ros to mqtt * Defines the data submitted from ros to mqtt
* Changes submitted data starting from the next data-package retreived from gazebo * Changes submitted data starting from the next data-package retreived from gazebo
...@@ -124,7 +62,7 @@ $ sudo ldconfig ...@@ -124,7 +62,7 @@ $ sudo ldconfig
* Channel: robotconfig * Channel: robotconfig
* allows to reconfigure velocity and planning-mode of the simulated robot * allows to reconfigure velocity and planning-mode of the simulated robot
* Protobuff-message format defined in https://git-st.inf.tu-dresden.de/ceti/ros/panda_simulation/-/blob/development/morse/proto/robotconfig.proto * Protobuff-message format defined in https://git-st.inf.tu-dresden.de/ceti/ros/panda_mqtt_connector/-/blob/development/morse/proto/robotconfig.proto
#### Parameters #### Parameters
...@@ -143,7 +81,7 @@ $ sudo ldconfig ...@@ -143,7 +81,7 @@ $ sudo ldconfig
#### General #### General
* Data for every link has its own mqtt-channel (defined in section bellow) * Data for every link has its own mqtt-channel (defined in section bellow)
* Protobuff-message format defined in https://git-st.inf.tu-dresden.de/ceti/ros/panda_simulation/-/blob/development/morse/proto/linkstate.proto * Protobuff-message format defined in https://git-st.inf.tu-dresden.de/ceti/ros/panda_mqtt_connector/-/blob/development/morse/proto/linkstate.proto
#### Channels #### Channels
...@@ -169,7 +107,7 @@ $ sudo ldconfig ...@@ -169,7 +107,7 @@ $ sudo ldconfig
#### Parameters #### Parameters
* MQTT-channel: trajectoryconfig * MQTT-channel: trajectoryconfig
* Protobuff-message format defined in https://git-st.inf.tu-dresden.de/ceti/ros/panda_simulation/-/blob/development/morse/proto/trajectory.proto * Protobuff-message format defined in https://git-st.inf.tu-dresden.de/ceti/ros/panda_mqtt_connector/-/blob/development/morse/proto/trajectory.proto
## Changelog ## Changelog
...@@ -180,7 +118,7 @@ $ sudo ldconfig ...@@ -180,7 +118,7 @@ $ sudo ldconfig
Integrates safetyzones via the SafetyAwarePlanner. Integrates safetyzones via the SafetyAwarePlanner.
start-command: start-command:
... ...
roslaunch panda_simulation simulation.launch roslaunch panda_mqtt_connector simulation.launch
... ...
To see through the boxes one have to set the alpha of the planning an environment (see picture). To see through the boxes one have to set the alpha of the planning an environment (see picture).
...@@ -190,10 +128,10 @@ $ sudo ldconfig ...@@ -190,10 +128,10 @@ $ sudo ldconfig
An example of one can add constraints to the planning and simulate it. start-command: An example of one can add constraints to the planning and simulate it. start-command:
start-command: start-command:
... ...
roslaunch panda_simulation simulation_constraint.launch roslaunch panda_mqtt_connector simulation_constraint.launch
... ...
This repository was extended with a ROS node that communicates with the _MoveIt!_ Planning Scene API. It makes sure that the motion planning pipeline avoids collision objects in the environment specified by the user in a separate directory (`~/.panda_simulation`) as _json_ files. This repository was extended with a ROS node that communicates with the _MoveIt!_ Planning Scene API. It makes sure that the motion planning pipeline avoids collision objects in the environment specified by the user in a separate directory (`~/.panda_mqtt_connector`) as _json_ files.
[Publishing a box at Panda's hand in _Gazebo_](https://erdalpekel.de/?p=123) [Publishing a box at Panda's hand in _Gazebo_](https://erdalpekel.de/?p=123)
......
config/panda_control.yaml deleted 100644 → 0
+ 0
− 42
View file @ 37947af3
# Publish joint states
joint_state_controller:
type: joint_state_controller/JointStateController
publish_rate: 100
panda_arm_controller:
type: position_controllers/JointTrajectoryController
joints:
- panda_joint1
- panda_joint2
- panda_joint3
- panda_joint4
- panda_joint5
- panda_joint6
- panda_joint7
constraints:
goal_time: 2.0
state_publish_rate: 100
panda_hand_controller:
type: position_controllers/JointTrajectoryController
joints:
- panda_finger_joint1
- panda_finger_joint2
state_publish_rate: 25
joint_position_controller:
type: panda_simulation/JointPositionController
arm_id: panda
joint_names:
- panda_joint1
- panda_joint2
- panda_joint3
- panda_joint4
- panda_joint5
- panda_joint6
- panda_joint7
gains: [
1, 1, 1, 1, 1, 1, 1
]
\ No newline at end of file
launch/box.launch deleted 100644 → 0
+ 0
− 9
View file @ 37947af3
<launch>
<param name="box_description" command="$(find xacro)/xacro --inorder $(find panda_simulation)/models/box.xacro"/>
<node name="box_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" args="robot_description:=box_description" />
<node name="spawn_object" pkg="gazebo_ros" type="spawn_model" args="-param box_description -urdf -model box"/>
<!-- launch node -->
<node pkg="panda_simulation" type="box_publisher_node" name="box_publisher_node" />
</launch>
\ No newline at end of file
launch/simulation.launch deleted 100644 → 0
+ 0
− 72
View file @ 37947af3
<launch>
<param name="robot_description" command="$(find xacro)/xacro $(find franka_description)/robots/panda_arm_hand.urdf.xacro" />
<!-- GAZEBO arguments -->
<arg name="paused" default="false" />
<arg name="use_sim_time" default="true" />
<arg name="gui" default="true" />
<arg name="headless" default="false" />
<arg name="debug" default="false" />
<arg name="load_gripper" default="true" />
<!--launch GAZEBO with own world configuration -->
<include file="$(find gazebo_ros)/launch/empty_world.launch">
<!-- <arg name="world_name" value="$(find robotics_assisted_tomography)/worlds/tomography.world"/> -->
<arg name="debug" value="$(arg debug)" />
<arg name="gui" value="$(arg gui)" />
<arg name="paused" value="$(arg paused)" />
<arg name="use_sim_time" value="$(arg use_sim_time)" />
<arg name="headless" value="$(arg headless)" />
</include>
<node name="rqt_console" pkg="rqt_console" type="rqt_console" />
<!-- <node name="rqt_logger_level" pkg="rqt_logger_level" type="rqt_logger_level" /> -->
<node name="spawn_urdf" pkg="gazebo_ros" type="spawn_model" args="-param robot_description -urdf -model panda" />
<!-- Load joint controller configurations from YAML file to parameter server -->
<rosparam file="$(find panda_simulation)/config/panda_control.yaml" command="load" />
<!-- load the controllers -->
<node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="joint_state_controller panda_arm_controller" />
<node if="$(arg load_gripper)" name="controller_spawner_hand" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="panda_hand_controller" />
<!-- convert joint states to TF transforms for rviz, etc -->
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" respawn="true" output="screen" />
<include file="$(find panda_moveit_config)/launch/planning_context.launch">
<arg name="load_robot_description" value="true" />
<arg name="load_gripper" value="$(arg load_gripper)" />
</include>
<include file="$(find panda_moveit_config)/launch/move_group.launch">
<arg name="load_gripper" value="$(arg load_gripper)" />
</include>
<group if="$(arg gui)">
<include file="$(find panda_moveit_config)/launch/moveit_rviz.launch" />
</group>
<node name="joint_state_desired_publisher" pkg="topic_tools" type="relay" args="joint_states joint_states_desired" />
<!-- launch robot control node for moveit motion planning -->
<node pkg="panda_simulation" type="robot_control_node" name="robot_control_node" output="screen" />
<!-- load (not start!) custom joint position controller -->
<node pkg="controller_manager" type="spawner" name="joint_position_launcher" args="--stopped joint_position_controller" />
<!-- run custom node for automatic intialization -->
<node pkg="panda_simulation" type="robot_state_initializer_node" name="robot_state_initializer_node" />
<!--<node name="move_group_interface_tutorial" pkg="panda_simulation" type="move_group_interface_tutorial" respawn="false" output="screen"/>-->
<node name="SafetyAwarePlanner" pkg="panda_simulation" type="SafetyAwarePlanner" respawn="false" output="screen">
<param name="message_redirect_rate" type="int" value="100" />
</node>
<!--<node name="motion_planning_api_tutorial" pkg="panda_simulation" type="motion_planning_api_tutorial" respawn="false" output="screen">
<rosparam command="load" file="$(find panda_moveit_config)/config/kinematics.yaml"/>
<param name="/planning_plugin" value="ompl_interface/OMPLPlanner"/>
<rosparam command="load" file="$(find panda_moveit_config)/config/ompl_planning.yaml"/>
</node>-->
</launch>
models/box.xacro deleted 100644 → 0
+ 0
− 40
View file @ 37947af3
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="simple_box">
<link name="object_base_link">
</link>
<joint name="object_base_joint" type="fixed">
<parent link="object_base_link"/>
<child link="object_link"/>
<axis xyz="0 0 1" />
<origin xyz="0 0 0" rpy="0 0 0"/>
</joint>
<link name="object_link">
<inertial>
<origin xyz="0 0 0" />
<mass value="1.0" />
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="100.0" iyz="0.0" izz="1.0" />
</inertial>
<visual>
<origin xyz="0 0 0"/>
<geometry>
<box size="0.04 0.04 0.08" />
</geometry>
</visual>
<collision>
<origin xyz="0 0 0"/>
<geometry>
<box size="0.04 0.04 0.08" />
</geometry>
</collision>
</link>
<gazebo reference="object_base_link">
<gravity>0</gravity>
</gazebo>
<gazebo reference="object_link">
<material>Gazebo/Blue</material>
</gazebo>
</robot>
\ No newline at end of file
scripts/docker-setup.sh deleted 100644 → 0
+ 0
− 33
View file @ 37947af3
#!/bin/bash
REPOSITORY_DIR=$(pwd)
WORKSPACE_DIR=/catkin_ws
DEPENDENCIES_DIR=$WORKSPACE_DIR/dependencies
APP_DIR=/root/.panda_simulation
# Install libfranka
mkdir $DEPENDENCIES_DIR && cd $DEPENDENCIES_DIR
git clone --recursive https://github.com/frankaemika/libfranka
cd libfranka
git checkout 0.5.0
git submodule update
mkdir build && cd build
cmake -DCMAKE_BUILD_TYPE=Release -DBUILD_TESTS=0 ..
cmake --build . -- -j$(nproc)
# Clone the franka_ros and panda_moveit_config repositories for simulating Panda robot
cd $WORKSPACE_DIR/src
git clone https://github.com/erdalpekel/panda_moveit_config.git
git clone --branch simulation https://github.com/erdalpekel/franka_ros.git
# Install package dependencies with rosdep
cd $WORKSPACE_DIR
rosdep install --from-paths src --ignore-src -y --skip-keys libfranka --skip-keys moveit_perception
# Clone the Visual-Studio-Code-ROS repository into the workspace directory /catkin_ws
cd $WORKSPACE_DIR
git clone git@github.com:erdalpekel/Visual-Studio-Code-ROS.git
mv Visual-Studio-Code-ROS .vscode
# create app directory for config files
mkdir $APP_DIR
\ No newline at end of file
src/joint_position_controller.cpp deleted 100644 → 0
+ 0
− 62
View file @ 37947af3
#include <controller_interface/controller.h>
#include <hardware_interface/joint_command_interface.h>
#include <pluginlib/class_list_macros.h>
#include <std_msgs/Float64MultiArray.h>
#include <vector>
namespace panda_simulation {
class JointPositionController : public controller_interface::Controller<hardware_interface::PositionJointInterface> {
bool init(hardware_interface::PositionJointInterface *hw, ros::NodeHandle &n) {
std::vector<std::string> joint_names;
if (!n.getParam("joint_names", joint_names)) {
ROS_ERROR("Could not read joint names from param server");
return false;
}
// retrieve gains
if (!n.getParam("gains", gains_vec_)) {
ROS_ERROR("Could not read joint gains from param server");
return false;
}
for (auto &joint_name : joint_names) {
joint_handles_.push_back(hw->getHandle(joint_name));
}
for (auto &joint_handle : joint_handles_) {
command_.push_back(joint_handle.getPosition());
}
sub_command_ = n.subscribe<std_msgs::Float64MultiArray>(std::string("command"), 1,
&JointPositionController::setCommandCallback, this);
return true;
}
void update(const ros::Time &time, const ros::Duration &period) {
for (size_t i = 0; i < joint_handles_.size(); i++) {
double error = command_.at(i) - joint_handles_.at(i).getPosition();
double commanded_effort = error * gains_vec_.at(i);
joint_handles_.at(i).setCommand(commanded_effort);
}
}
void setCommandCallback(const std_msgs::Float64MultiArrayConstPtr &msg) { command_ = msg->data; }
void starting(const ros::Time &time) {}
void stopping(const ros::Time &time) {}
private:
std::vector<hardware_interface::JointHandle> joint_handles_;
std::vector<double> gains_vec_;
std::vector<double> command_;
ros::Subscriber sub_command_;
};
PLUGINLIB_EXPORT_CLASS(panda_simulation::JointPositionController, controller_interface::ControllerBase);
} // namespace panda_simulation
\ No newline at end of file
src/robot_state_initializer.cpp deleted 100644 → 0
+ 0
− 83
View file @ 37947af3
#include <controller_manager_msgs/SwitchController.h>
#include <ros/ros.h>
#include <std_msgs/Float64MultiArray.h>
void initSystemVariables(ros::NodeHandle node_handle)
{
node_handle.setParam("new_trajectory_available", false);
node_handle.setParam("loop_trajectory", true);
}
int main(int argc, char **argv) {
ros::init(argc, argv, "robot_state_initializer_node");
ros::NodeHandle node_handle;
initSystemVariables(node_handle);
std::vector<double> panda_ready_state{0, -0.785, 0, -2.356, 0, 1.571, 0.785};
// define variables
std::string joint_position_command_topic{"/joint_position_controller/command"},
controller_manager_switch_topic{"/controller_manager/switch_controller"};
ros::ServiceClient switch_controller_client =
node_handle.serviceClient<controller_manager_msgs::SwitchController>(controller_manager_switch_topic);
ros::Publisher joint_position_publisher =
node_handle.advertise<std_msgs::Float64MultiArray>(joint_position_command_topic, 1);
// sleep for 2 seconds in order to make sure that the system is up and running
ros::Duration(2.0).sleep();
// 1. switch from default joint trajectory controller to custom position controller
std::string panda_arm_controller{"panda_arm_controller"}, panda_hand_controller{"panda_hand_controller"},
joint_position_controller{"joint_position_controller"};
std::vector<std::string> stop_controllers{panda_arm_controller, panda_hand_controller};
std::vector<std::string> start_controllers{joint_position_controller};
controller_manager_msgs::SwitchController srv_switch_controller;
srv_switch_controller.request.stop_controllers = stop_controllers;
srv_switch_controller.request.start_controllers = start_controllers;
srv_switch_controller.request.strictness = controller_manager_msgs::SwitchControllerRequest::BEST_EFFORT;
// housekeeping for logging
std::ostringstream stream_start_controllers, stream_stop_controllers;
for (const auto &elem : start_controllers) {
stream_start_controllers << elem << ", ";
}
for (const auto &elem : stop_controllers) {
stream_stop_controllers << elem << ", ";
}
if (switch_controller_client.call(srv_switch_controller)) {
ROS_INFO_STREAM("Success switching controllers from " << stream_stop_controllers.str() << " to "
<< stream_start_controllers.str());
} else {
ROS_WARN_STREAM("Error switching controllers from " << stream_stop_controllers.str() << " to "
<< stream_start_controllers.str());
return -1;
}
// 2. publish the desired joint position to the custom controller
std_msgs::Float64MultiArray command_msg;
command_msg.data = panda_ready_state;
joint_position_publisher.publish(command_msg);
// sleep for 1 seconds in order to make sure that the controller finishes moving the robot
ros::Duration(1.0).sleep();
// 3. Restore default controllers
std::swap(stop_controllers, start_controllers);
srv_switch_controller.request.stop_controllers = stop_controllers;
srv_switch_controller.request.start_controllers = start_controllers;
if (switch_controller_client.call(srv_switch_controller)) {
ROS_INFO_STREAM("Success switching controllers from " << stream_stop_controllers.str() << " to "
<< stream_start_controllers.str());
} else {
ROS_WARN_STREAM("Error switching controllers from " << stream_stop_controllers.str() << " to "
<< stream_start_controllers.str());
return -1;
}
ros::spin();
return 0;
}
\ No newline at end of file
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment